Covariance Steering with Optimal Risk Allocation

Joshua Pilipovsky^* Panagiotis Tsiotras^$\dagger$ ^* J. Pilipovsky is a Graduate student at the School of Aerospace Engineering, Georgia Institute of Technology, Atlanta, GA 30332-0150, USA. Email: [email protected]^$\dagger$ P. Tsiotras is the David & Lewis Chair and Professor at the School of Aerospace Engineering and the Institute for Robotics & Intelligent Machines, Georgia Institute of Technology, Atlanta, GA 30332-0150, USA. Email: [email protected]

Abstract

This paper extends the optimal covariance steering problem for linear stochastic systems subject to chance constraints so as to account for an optimal allocation of the risk. Previous works have assumed a uniform risk allocation in order to cast the optimal control problem as a semi-definite program (SDP), which can be solved efficiently using standard SDP solvers. An Iterative Risk Allocation (IRA) formalism is used to solve the optimal risk allocation problem for covariance steering using a two-stage approach. The upper-stage of IRA optimizes the risk, which is a convex problem, while the lower-stage optimizes the controller with the new constraints. The process is applied iteratively until the optimal risk allocation that achieves the lowest total cost is obtained. The proposed framework results in solutions that tend to maximize the terminal covariance, while still satisfying the chance constraints, thus leading to less conservative solutions than previous methodologies. In this work, we consider both polyhedral and cone state chance constraints. Finally, we demonstrate the approach to a spacecraft rendezvous problem and compare the results with other competing approaches.

I Introduction

In this paper we address the problem of finite-horizon stochastic optimal control of a discrete linear time-varying (LTV) system with white-noise Gaussian diffusion. The control task is to steer the state from an initial Gaussian distribution to a final Gaussian distribution with known statistics. In addition to the boundary conditions, we consider chance constraints that restrict the probability of violating state constraints to be less than a certain threshold. In general, hard state constraints are difficult to impose in stochastic systems because the noise can be unbounded, so chance constraints are typically used to deal with this problem by imposing a small, but finite, probability of violating the constraints. In the literature, one encounters two kinds of chance constraints; individual chance constraints and joint chance constraints [1]. Individual chance constraints limit the probability of violating each constraint, while joint chance constraints limit the probability of violating any constraint over the whole time horizon. In this paper, we consider the case of joint chance constraints, because they are a more natural choice for most applications of interest.

Control of stochastic systems can be best formulated as a problem of controlling the distribution of trajectories over time. Moreover, Gaussian distributions are completely characterized by their first and second moments, so the control problem can be thought of as one of steering the mean and the covariance to the desired terminal values. The problem of covariance control has a history dating back to the ’80s, with the works of Hotz and Skelton [2, 3]. Much of the early work focused on the infinite horizon problem, where the state covariance asymptotically approaches its terminal value. Only recently has the finite-horizon problem drawn attention, with much of the early work focusing on the so-called covariance steering (CS) problem, namely, the problem of steering an initial distribution to a final distribution at a specific final time step subject to linear time-varying (LTV) dynamics. The problem can be thought of as a linear-quadratic Gaussian (LQG) problem with a condition on the terminal covariance [4]. Moreover, it has been shown that the finite-horizon covariance steering controller can be constructed as a state-feedback controller, and the problem can be formulated either as a convex program [4, 5], or as the solution of a pair of Lyapunov differential equations coupled through their boundary conditions [6, 7]. Alternatively, for certain special cases one can solve the CS problem directly by solving an LQ stochastic problem with a particular choice of cost weights [8]. Other approaches [9, 10] use an affine-disturbance feedback controller having two components, one that steers the mean state and the other that steers the covariance.

Steering the covariance is related to the theory of steering marginal distributions, which has a long history, stemming from the problem of Schrödinger bridges and optimal mass transport [7, 11, 12, 13]. Most recent work on covariance steering has focused on incorporating physical constraints on the system, such as state chance constraints [14], obstacles in path-planning environments [10], input hard constraints [15], incomplete state information [16], and extensions in the context of stochastic model predictive control [17] and nonlinear systems [18, 19, 20].

In this work, we first review the formalism for the Covariance Steering Chance Constraint (CSCC) problem, to account for optimal risk allocation. By risk allocation we mean allocating the probability of violating each individual chance constraint at each time step. For example, if there are $M$ chance constraints and $N$ time steps, there would be $NM$ total allocations for the whole problem. Previous works [8, 14, 10, 15, 17] have assumed a constant risk allocation, so that the resulting problem can be turned into a semi-definite program (SDP). Here, however, we adopt an iterative two-stage algorithm that optimizes the risk distribution over all time steps, and subsequently optimizes the controller by solving a SDP. Other works have tried to optimize the risk using techniques such as ellipsoidal relaxation[21] and particle control[22]. However, ellipsoidal relaxation techniques are overly conservative and lead to highly suboptimal solutions. Particle control methods are computationally too demanding, since the number of decision variables grows with the number of particles. The two-stage risk allocation scheme proposed in this work is computed iteratively until the cost is within a given tolerance of the minimum, from which we get the optimal risk allocation for the problem, as well as the optimal controller.

The contributions of this work are as follows: First, we propose the first work that solves the covariance steering problem while optimally allocating the risk of violating chance constraints. Second, we extend previous similar results in the literature that only deal with polyhedral constraints to the case of cone constraints. Third, we provide a result of independent interest for convexifying the chance constraints for the case of conical regions. Finally, and in order to illustrate the proposed risk allocation algorithm, we use as an example a rendezvous problem between two spacecraft, in which the approaching spacecraft’s control inputs are constrained to be within some bounds, while its position should remain within a predetermined LOS region during the whole maneuver. Both polyhedral and cone LOS constraints are investigated and compared.

This work extends the preliminary results of [23] along several directions. First, while in [23] only polyhedral chance constraints were handled, in this work we also treat the important – and much more difficult – case of cone constraints. Indeed, in many applications the constraints are given in the form of a conical region (e.g., line-of-sight (LOS) constraints). Approximating such cone constraints with many intersecting hyperplanes would make the problem computationally expensive and quite challenging in terms of achieving high accuracy approximations. Second, we also present a way to approximate such cone chance constraints as special cases of general quadratic constraints in terms of two-sided polyhedral constraints. Third, we apply this formulation to the case of LOS cone chance constraints, and compare with a polyhedral approximation. Fourth, we present a geometric relaxation of the cone chance constraints, which is more natural in real-life applications. Finally, we compare our results with stochastic MPC-type approaches.

The paper is structured as follows: In Section II we define the general stochastic optimal control problem for steering a distribution from an initial Gaussian to a terminal Gaussian with joint state chance constraints. In Section III we review the two-stage risk allocation formalism, and formulate the SDP for the optimal controller as well as the proposed iterative risk allocation algorithm. In Section IV we present two different convex relaxations of quadratic chance constraints, one in terms of a two-sided linear constraint relaxation, and the other based on a geometrical construction. Finally, in Section V we implement the theory to the spacecraft rendezvous and docking problem with both polyhedral and cone chance constraints, and compare with stochastic MPC methods.

II Problem Statement

We consider the following discrete-time stochastic time-varying system subject to noise

~{}x_{k+1}=A_{k}x_{k}+B_{k}u_{k}+D_{k}w_{k},

(1)

where $x\in\mathbb{R}^{n},u\in\mathbb{R}^{m}$ , with time steps $k=0,\ldots,N-1$ , where $N$ representing the finite horizon. The uncertainty $w\in\mathbb{R}^{r}$ is a zero-mean white Gaussian noise with unit covariance, i.e., $\mathbb{E}[w_{k}]=0$ and $\mathbb{E}[w_{k_{1}}w_{k_{2}}^{\intercal}]=I_{r}\delta_{k_{1},k_{2}}$ . Additionally, we assume that $\mathbb{E}[x_{k_{1}}w_{k_{2}}^{\intercal}]=0$ , for $0\leq k_{1}\leq k_{2}\leq N$ . The initial state $x_{0}$ is a random vector drawn from the normal distribution

~{}x_{0}\sim\mathcal{N}(\mu_{0},\Sigma_{0}),

(2)

where $\mu_{0}\in\mathbb{R}^{n}$ is the initial state mean and $\Sigma_{0}\in\mathbb{R}^{n\times n}>0$ is the initial state covariance. The objective is to steer the trajectories of (1) from the initial distribution (2) to the terminal distribution

~{}x_{f}\sim\mathcal{N}(\mu_{f},\Sigma_{f}),

(3)

where $\mu_{f}\in\mathbb{R}^{n}$ and $\Sigma_{f}>0$ are the state mean and covariance at time $N$ , respectively. The cost function to be minimized is

~{}J(u_{0},\ldots,u_{N-1})\coloneqq\mathbb{E}\bigg{[}\sum_{k=0}^{N-1}x_{k}^{\intercal}Q_{k}x_{k}+u_{k}^{\intercal}R_{k}u_{k}\bigg{]},

(4)

where $Q_{k}\geq 0$ and $R_{k}>0$ for all $k=0,\ldots,N-1$ . Additionally, and over the whole horizon, we impose the following joint chance constraint that limits the probability of state violation to be less than a pre-specified threshold, i.e.,

~{}\mathbb{P}\bigg{(}\bigwedge_{k=0}^{N}x_{k}\notin\mathcal{X}\bigg{)}\leq\Delta,

(5)

where $\mathbb{P}(\cdot)$ denotes the probability of an event, $\mathcal{X}\subset\mathbb{R}^{n}$ is the state constraint set, and $\Delta\in(0,0.5]$ .

Remark 1: We assume that the system (1) is controllable, that is, for any $x_{0},x_{f}\in\mathbb{R}^{n}$ , and no noise ( $w_{k}\equiv 0,~{}k=0,\ldots,N-1$ ), there exists a sequence of control inputs $\{u_{k}\}_{k=0}^{N-1}$ that steer the system from $x_{0}$ to $x_{f}$ .

First, we provide an alternative description of the system (1) in order to solve the problem at hand. Using [8, 14, 10, 15, 17], we can reformulate (1) as

~{}X=\mathcal{A}x_{0}+\mathcal{B}U+\mathcal{D}W,

(6)

where $X\coloneqq[x_{0}^{\intercal},...x_{N}^{\intercal}]^{\intercal}\in\mathbb{R}^{(N+1)n},U\coloneqq[u_{0}^{\intercal},...u_{N-1}^{\intercal}]^{\intercal}\in\mathbb{R}^{Nm}$ , and $W\coloneqq[w_{0}^{\intercal},...,w_{N-1}^{\intercal}]^{\intercal}\in\mathbb{R}^{Nr}$ are the state, input, and disturbance sequences, respectively. The matrices $\mathcal{A},\mathcal{B}$ , and $\mathcal{D}$ are defined accordingly [8]. Using this notation, we can write the cost function compactly as

~{}J(U)=\mathbb{E}[X^{\intercal}\bar{Q}X+U^{\intercal}\bar{R}U],

(7)

where $\bar{Q}$ and $\bar{R}$ are defined accordingly. Note that since $Q_{k}\geq 0$ and $R_{k}>0$ for all $k=0,\ldots,N-1$ , it follows that $\bar{Q}\geq 0$ and $\bar{R}>0$ . The initial and terminal conditions (2) and (3) can be written as

~{}\mu_{0}=E_{0}\mathbb{E}[X],\qquad\Sigma_{0}=E_{0}\Sigma_{X}E_{0},

(8)

and

~{}\mu_{f}=E_{N}\mathbb{E}[X],\qquad\Sigma_{f}=E_{N}\Sigma_{X}E_{n},

(9)

where $\Sigma_{X}\coloneqq\mathbb{E}[XX^{\intercal}]-\mathbb{E}[X]\mathbb{E}[X]^{\intercal}$ , and $E_{k}\coloneqq[0_{n,kn},I_{n},0_{n,(N-k)n}]$ picks out the $k$ th component of a vector. Consequently, the state chance constraints (5) can be written as

~{}\mathbb{P}\bigg{(}\bigwedge_{k=1}^{N}E_{k}X\notin\mathcal{X}\bigg{)}\leq\Delta.

(10)

In summary, we wish to solve the following stochastic optimal control problem.

Problem 1 : Given the system (6), find the optimal control sequence $U^{*}\coloneqq U^{*}_{N-1}$ that minimizes the objective function (7), subject to the initial state (8), terminal state (9), and the state chance constraints (10).

III Chance Constrained Covariance Steering with Risk Allocation

III-A Lower-Stage Covariance Steering

Borrowing from the work in [10], we adopt the control policy

~{}u_{k}=v_{k}+K_{k}y_{k},

(11)

where $v_{k}\in\mathbb{R}^{m},K_{k}\in\mathbb{R}^{m\times n}$ , and $y_{k}\in\mathbb{R}^{n}$ is given by


$\displaystyle y_{k+1}$	$\displaystyle=A_{k}y_{k}+D_{k}w_{k},$	(12a)
$\displaystyle y_{0}$	$\displaystyle=x_{0}-\mu_{0}.$	(12b)

Remark 2: The proposed control scheme (11)-(12) leads to a convex programming formulation of Problem 1 as follows.

Using (11)-(12), we can write the control sequence as

~{}U=V+KY,

(13)

where $Y:=\mathcal{A}y_{0}+\mathcal{D}W\in\mathbb{R}^{(N+1)n},V\coloneqq[v_{0}^{\intercal},\ldots,v_{N-1}^{\intercal}]^{\intercal}\in\mathbb{R}^{Nm}$ and $K\in\mathbb{R}^{Nm\times(N+1)n}$ a matrix containing the gains $K_{k}$ . It follows that the dynamics can be decoupled into a mean and error state as follows

	$\displaystyle\bar{X}$	$\displaystyle=\mathbb{E}[X]=\mathcal{A}\mu_{0}+\mathcal{B}V,$		(14)
	$\displaystyle\tilde{X}$	$\displaystyle=X-\bar{X}=(I+\mathcal{B}K)Y.$		(15)

Additionally, by noting that the cost function (7) can be decoupled into a mean cost $J_{\mu}=\bar{X}^{\intercal}\bar{Q}\bar{X}+\bar{U}^{\intercal}\bar{R}\bar{U}$ and a covariance cost $J_{\Sigma}=\textrm{tr}(Q\Sigma_{X})+\textrm{tr}(Q\Sigma_{U})$ , the cost function takes the form [10]

	$\displaystyle~{}J(V,K)=$	$\displaystyle(\mathcal{A}\mu_{0}+\mathcal{B}V)^{\intercal}\bar{Q}(\mathcal{A}\mu_{0}+\mathcal{B}V)+V^{\intercal}\bar{R}V$
	$\displaystyle+\ \text{tr}\Big{[}\big{(}$	$\displaystyle(I+\mathcal{B}K)^{\intercal}\bar{Q}(I+\mathcal{B}K)+K^{\intercal}\bar{R}K\big{)}\Sigma_{Y}\Big{]},$		(16)

where $\Sigma_{Y}\coloneqq\mathcal{A}\Sigma_{0}\mathcal{A}^{\intercal}+\mathcal{D}\mathcal{D}^{\intercal}$ . The terminal constraints can be reformulated as


$\displaystyle\mu_{f}$	$\displaystyle=E_{N}(\mathcal{A}\mu_{0}+\mathcal{B}V),$	(17a)
$\displaystyle\Sigma_{f}$	$\displaystyle=E_{N}(I+\mathcal{B}K)\Sigma_{Y}(I+\mathcal{B}K)^{\intercal}E_{N}^{\intercal}.$	(17b)

Qualitatively speaking, $V$ steers the mean of the system to $\mu_{f}$ , while $K$ steers the covariance to $\Sigma_{f}$ . In order to make the problem convex, we relax the terminal covariance constraint (17b) to $\Sigma_{f}\geq E_{N}(I+\mathcal{B}K)\Sigma_{Y}(I+\mathcal{B}K)^{\intercal}E_{N}^{\intercal}$ , which can be written as the linear matrix inequality (LMI)

~{}\begin{bmatrix}\Sigma_{f}&E_{N}(I+\mathcal{B}K)\Sigma_{Y}^{1/2}\\ \Sigma_{Y}^{1/2}(I+\mathcal{B}K)^{\intercal}E_{N}^{\intercal}&I\end{bmatrix}\geq 0.

(18)

III-B Polyhedral Chance Constraints

When dealing with the risk allocation problem, it is customary to assume that the state constraint set $\mathcal{X}$ is a convex polytope $\mathcal{X}^{p}$ , so that

~{}\mathcal{X}^{p}\coloneqq\bigcap_{j=1}^{M}\{x:\alpha_{j}^{\intercal}x\leq\beta_{j}\},

(19)

where $\alpha_{j}\in\mathbb{R}^{n}$ and $\beta_{j}\in\mathbb{R}$ . Under this assumption, the probability of violating the state constraints (10) can be written as

~{}\mathbb{P}\bigg{(}\bigwedge_{k=1}^{N}\bigwedge_{j=1}^{M}\alpha_{j}^{\intercal}E_{k}X>\beta_{j}\bigg{)}\leq\Delta.

(20)

Equation (20) represents the objective that the joint probability of violating any of the $M$ state constraints over the horizon $N$ is less than or equal to $\Delta$ . Using Boole’s Inequality [24, 25], one can decompose a joint chance constraint into individual chance constraints as follows

~{}\mathbb{P}\big{(}\alpha_{j}^{\intercal}E_{k}X\leq\beta_{j}\big{)}\geq 1-\delta_{k}^{j},~{}~{}~{}k=1,\ldots,N,~{}j=1,\ldots,M.\textsl{}

(21)

with

~{}\sum_{k=1}^{N}\sum_{j=1}^{M}\delta_{k}^{j}\leq\Delta,

(22)

where each $\delta_{k}^{j}$ represents the probability of violating the $j$ th constraint at time step $k$ . Notice that the probability in (21) is of a random variable with mean $\alpha_{j}^{\intercal}E_{k}\bar{X}$ and covariance $\alpha_{j}^{\intercal}E_{k}\Sigma_{X}E_{k}^{\intercal}\alpha_{j}$ . Thus, (21) can be equivalently written as

~{}\Phi\Bigg{(}\frac{\beta_{j}-\alpha_{j}^{\intercal}E_{k}\bar{X}}{\sqrt{\alpha_{j}^{\intercal}E_{k}\Sigma_{X}E_{k}^{\intercal}\alpha_{j}}}\Bigg{)}\geq 1-\delta_{k}^{j},

(23)

where $\Phi(\cdot)$ denotes the cumulative distribution function of the standard normal distribution. Simplifying (23) and noting that $\Sigma_{X}=(I+\mathcal{B}K)\Sigma_{Y}(I+\mathcal{B}K)^{\intercal}$ yields

	$\displaystyle~{}\alpha_{j}^{\intercal}$	$\displaystyle E_{k}(\mathcal{A}\mu_{0}+\mathcal{B}V)$
		$\displaystyle+\\|\Sigma_{Y}^{1/2}(I+\mathcal{B}K)^{\intercal}E_{k}^{\intercal}\alpha_{j}\\|\Phi^{-1}(1-\delta_{k}^{j})\leq\beta_{j}.$		(24)

Remark 3: Since $\Sigma_{0}>0$ , it follows that $\Sigma_{Y}>0$ , and $\Sigma_{Y}^{1/2}$ in (III-B) can be computed from its Cholesky decomposition.

The expression in (III-B) gives $NM$ inequality constraints for the optimization problem. In summary, Problem 1 is converted into a convex programming problem.

Problem 2 : Given the system (14) and (15), find the optimal control sequences $V^{*}$ and $K^{*}$ that minimize the cost function (III-A) subject to the terminal state constraints (17a) and (18), and the individual chance constraints (III-B).

Remark 4: Note that it is not possible to decouple the mean and covariance controllers in the presence of chance constraints, because of (III-B).

III-C Risk Allocation Optimization

Since $\delta_{k}^{j}$ are decision variables in (III-B), the constraints are bilinear, which makes it difficult to solve this problem. As mentioned previously, in order to transform Problem 2 to a more tractable form, the allocation of the risk levels $\delta_{k}^{j}$ may be assumed to be fixed to some pre-specified values, usually uniformly. In this case, $\delta_{k}^{j}$ are no longer decision variables and the problem can be efficiently solved as an SDP. However, a better approach is to allocate $\delta_{k}^{j}$ concurrently when solving the optimization Problem 2, so as to minimize the total cost. This gives rise to a natural two-stage optimization framework [26].

Following the approach in [26], the upper stage optimization finds the optimal risk allocation $\delta\coloneqq[\delta_{1}^{1},\delta_{1}^{2},\ldots,\delta_{N}^{M-1},\delta_{N}^{M}]\in\mathbb{R}^{N\!M}$ , and the lower stage solves the CS problem for the optimal controller $U^{*}=U^{*}_{N-1}$ given the risk allocation $\delta$ from the upper-stage.

Let the value of the objective function after the lower-stage optimization for a given risk allocation $\delta$ be $J^{*}$ , that is,

~{}J^{*}(\delta)=\min_{V,K}J(V,K),

(25)

where $J(V,K)$ is given in (III-A). The upper-stage optimization problem can then be formulated as follows.

Problem 3 (Risk Allocation):

	$\displaystyle\min_{\delta}J^{*}(\delta),$	(26)
such that	$\displaystyle\quad\sum_{k=1}^{N}\sum_{j=1}^{M}\delta_{k}^{j}\leq\Delta,$	(27)
	$\displaystyle\qquad\delta_{k}^{j}>0,$	(28)

As shown in [26], Problem 3 is a convex optimization problem, given that the objective function $J(V,K)$ is convex, and $\Delta\in(0,0.5]$ .

III-D Iterative Risk Allocation Motivation

Even though we have formulated the solution of Problem 2 as a two-stage optimization problem, it is not clear yet how to solve Problem 3 efficiently in order to determine the optimal risk allocation. To gain insight into the solution, we first state a theorem from [26] about the monotonicity of $J^{*}(\delta)$ .

Theorem 1.

The optimal cost from solving Problem 2 is a monotonically decreasing function in $\delta_{k}^{j}$ , that is,

~{}\frac{\partial J^{*}}{\partial\delta_{k}^{j}}\leq 0,\quad k=1,\ldots,N,~{}j=1,\ldots,M.

(29)

Proof.

See [26]. ∎

In the following section, we use this theorem to create an iterative algorithm that solves Problem 3 by lowering the cost at each iteration. The main idea is that, by carefully increasing the risk allocations $\delta_{k}^{j}$ , Theorem 1 guarantees that the optimal cost will be reduced with each successive iteration. As such, the algorithm lowers the risk, equivalently tightens the constraints, for the constraints that are too conservative, and increases the risk, equivalently loosens the constraints, for the constraints that are already active. The following remark introduces this idea and defines active and inactive constraints in the context of risk allocation.

Remark 5: The chance constraints can be written in yet another form that will prove useful below. Starting from (III-B), notice that we can write the chance constraints as

~{}\delta_{k}^{j}\geq 1-\Phi\Bigg{(}\frac{\beta_{j}-\alpha_{j}^{\intercal}E_{k}\bar{X}^{*}}{\|\Sigma_{Y}^{1/2}(I+\mathcal{B}K^{*})^{\intercal}E_{k}^{\intercal}\alpha_{j}\|}\Bigg{)}\eqqcolon\bar{\delta}_{k}^{j}.

(30)

The quantity $\bar{\delta}_{k}^{j}$ represents the true risk experienced by the optimal trajectories, i.e, when using $(V^{*},K^{*})$ . Clearly, the risk we have selected does not need to be equal to the actual risk once the optimization is completed. When these values are equal we will say that the constraint (30) is active, and is inactive otherwise. Good solutions correspond to cases when the true risk is within a small margin of the allocated risk. Many $\delta_{k}^{j}$ having values smaller than their true counterparts would imply an overly conservative solution.

III-E Iterative Risk Allocation Algorithm

We can exploit Theorem 1 in the context of CS to create an iterative risk allocation algorithm that simultaneously finds the optimal risk allocation $\delta^{*}$ and the optimal control pair $(V^{*},K^{*})$ . To this end, suppose we start with some feasible risk allocation $\delta_{k(i)}^{j}$ , for all $k,j$ , where $i$ denotes the iteration number. Using this risk allocation, we then solve Problem 2 to get the optimal controller $(V_{(i)}^{*},K_{(i)}^{*})$ , which corresponds to the optimal mean trajectory $\bar{X}_{(i)}^{*}$ at iteration $i$ . Next, we construct a new risk allocation $\delta_{k(i)}^{j\prime}$ as follows: for all $k,j$ such that $\delta_{k(i)}^{j}$ is active, we keep the corresponding allocation the same, i.e, $\delta_{k(i)}^{j\prime}=\delta_{k(i)}^{j}$ . However, for all $k,j$ such that $\delta_{k(i)}^{j}$ is inactive we let $\delta_{k(i)}^{j\prime}<\delta_{k(i)}^{j}$ . This corresponds to tightening the constraints. Since this new risk allocation is smaller, and since $\Phi^{-1}(z)$ is a monotonically increasing function, it follows that $\Phi^{-1}(1-\delta_{k(i)}^{j\prime})>\Phi^{-1}(1-\delta_{k(i)}^{j})$ . Furthermore, this implies that

	$\displaystyle~{}\alpha_{j}^{\intercal}E_{k}\bar{X}_{(i)}^{*}$	$\displaystyle<\beta_{j}-\\|\Sigma_{Y}^{1/2}(I+\mathcal{B}K_{(i)}^{*})^{\intercal}E_{k}^{\intercal}\alpha_{j}\\|\Phi^{-1}(1-\delta_{k(i)}^{j\prime})$
		$\displaystyle<\beta_{j}-\\|\Sigma_{Y}^{1/2}(I+\mathcal{B}K_{(i)}^{*})^{\intercal}E_{k}^{\intercal}\alpha_{j}\\|\Phi^{-1}(1-\delta_{k(i)}^{j}).$		(31)

Constraint (III-E) ensures that the optimal solution for $\delta_{(i)}$ is feasible for $\delta_{(i)}^{\prime}$ . Furthermore, since $\delta_{k(i)}^{j\prime}<\delta_{k(i)}^{j}$ , it follows that $\mathcal{R}(\delta^{\prime})\subseteq\mathcal{R}(\delta)$ , so the optimal solution for $\delta_{(i)}$ is also the optimal solution for $\delta_{(i)}^{\prime}$ as well, hence $J^{*}(\delta^{\prime})=J^{*}(\delta)$ .

Next, we construct a new risk allocation $\delta_{k(i+1)}^{j}$ from $\delta_{k(i)}^{j\prime}$ as follows. For all $k,j$ such that $\delta_{k(i)}^{j\prime}$ is inactive, leave the new risk allocation the same. For all $k,j$ such that $\delta_{k(i)}^{j\prime}$ is active, let $\delta_{k(i+1)}^{j}>\delta_{k(i)}^{j\prime}$ , which corresponds to relaxing the constraints. Following the same logic, Theorem 1 implies that $J^{*}(\delta_{(i)}^{\prime})\geq J^{*}(\delta_{(i+1)})$ . Thus, we have laid out an iterative scheme for a sequence of risk allocations $\{\delta_{(0)},\delta_{(1)},\ldots,\delta_{(i)}\}$ that continually lowers the optimal cost.

This leads to Algorithm 1 that solves the optimal risk allocation for the CS problem subject to chance constraints. Note that the algorithm is initialized with a constant risk allocation. To tighten the inactive constraints in Line 9, the corresponding risk is scaled by a parameter $0<\rho<1$ that weighs the current risk with the true risk from that solution. Additionally, to loosen the active constraints in Line 13, the corresponding risk is increased proportionally to the residual risk remaining.

Input:

\delta_{k}^{j}\leftarrow\Delta/(NM),\epsilon,\rho

Output:

\delta^{*},J^{*},V^{*},K^{*}

1 while $|J^{*}-J^{*}_{\rm prev}|>\epsilon$ do

J^{*}_{\rm prev}\leftarrow J^{*}

3 Solve Problem 2 with current

\delta

to obtain

\bar{\delta}

\hat{N}\leftarrow

number of indices where constraint is active

5 if $\hat{N}=0$ or $\hat{N}=MN$ then

6 break;

8 end if

9 foreach $(k,j)$ such that $j$ th constraint at $k$ th time step is inactive do

\delta_{k}^{j}\leftarrow\rho\delta_{k}^{j}+(1-\rho)\bar{\delta}_{k}^{j}

11 end foreach

\delta_{\text{res}}\leftarrow\Delta-\sum_{k=1}^{N}\sum_{j=1}^{M}\delta_{k}^{j}

13 foreach $(k,j)$ such that $j$ th constraint at $k$ th time step is active do

\delta_{k}^{j}\leftarrow\delta_{k}^{j}+\delta_{\text{res}}/\hat{N}

15 end foreach

17 end while

Algorithm 1 Iterative Risk Allocation CS

IV Cone Chance Constraints

In many engineering applications polytopic constraints such as (19) are not realistic. Most often, the constraints have the form of a convex cone, namely, the feasible region is characterized by

~{}\mathcal{X}^{c}:=\{x\in\mathbb{R}^{n}:\|Ax+b\|_{2}\leq c^{\intercal}x+d\}.

(32)

Cone constraints such as (32) are more realistic, as they better describe the feasible space. As with the case of a polyhedral feasible state space $\mathcal{X}^{p}$ , we want the state to be inside $\mathcal{X}^{c}$ throughout the whole time horizon. However, since the dynamics are stochastic, this assumption is relaxed to the condition that the probability that the state is not inside this set is less than or equal to $\Delta$ . In the context of convex cone state constraints, this condition becomes


$\displaystyle\!\mathbb{P}(\\|Ax_{k}+b\\|_{2}\leq c^{\intercal}$	$\displaystyle x_{k}+d)\geq 1-\delta_{k},~{}~{}k=1,\ldots,N,$	(33a)
$\displaystyle\sum_{k=1}^{N}\delta_{k}$	$\displaystyle\leq\Delta.$	(33b)

Remark 6: Although the set $\mathcal{X}^{c}$ is convex, the chance constraint $\mathbb{P}(x\in\mathcal{X}^{c})\geq 1-\delta$ may not be convex. Specifically, for large $\delta_{k}$ , it is possible that the chance constraint (33a) is non-convex [27].

Since there is no guarantee that (33) will be a convex constraint, we need to make a convex approximation so that (33) holds for all $\Delta\in(0,0.5]$ .

IV-A Two-Sided Approximation of Cone Constraints

In order to approximate the cone chance constraint (33a), we first replace the cone constraint in (33a) with the quadratic chance constraint

\mathbb{P}(\|Ax_{k}+b\|_{2}\leq c^{\intercal}\bar{x}_{k}+d)\geq 1-\delta_{k},~{}~{}k=1,\ldots,N.

(34)

Remark 7: The chance constraint (34) is a relaxation of the original chance constraint (33a). The proof of this result is given in Appendix A.

Note that (34) can be equivalently written as

~{}\mathbb{P}\left[\left(\sum_{i=1}^{n}(a_{i}^{\intercal}x_{k}+b_{i})^{2}\right)^{1/2}\leq\kappa_{k}\right]\geq 1-\delta_{k},~{}~{}k=1,\ldots,N,

(35)

where $\kappa_{k}:=c^{\intercal}\bar{x}_{k}+d$ and $a_{i}^{\intercal}$ denotes the $i$ th row of $A$ . Squaring both sides of (35) and letting $\psi_{i,k}:=a_{i}^{\intercal}x_{k}+b_{i}$ yields

\mathbb{P}\left(\sum_{i=1}^{n}\psi_{i,k}^{2}\leq\kappa_{k}^{2}\right)\geq 1-\delta_{k},~{}~{}k=1,\ldots,N.

(36)

The following proposition enables the conversion of a quadratic constraint of the form (36) to a collection of two-sided (absolute value) constraints. To simplify the notation, below we drop the subscript $k$ from the corresponding expressions.

Proposition 1.

The quadratic constraint

\mathbb{P}\left(\sum_{i=1}^{n}\psi_{i}^{2}\leq\kappa^{2}\right)\geq 1-\delta,

(37)

is satisfied if the following constraints are satisfied


	$\displaystyle\mathbb{P}(\|\psi_{i}\|\leq f_{i})\geq 1-\beta_{i}\delta,\quad i=1,\ldots,n,$		(38a)
	$\displaystyle\sum_{i=1}^{n}f_{i}^{2}\leq\kappa^{2},$		(38b)
	$\displaystyle\sum_{i=1}^{n}\beta_{i}=1,$		(38c)

for some non-negative $f_{1},f_{2},\ldots,f_{n}$ and $\beta_{1},\beta_{2},\ldots,\beta_{n}$ .

Proof.

Since $|\psi_{i}|\leq f_{i}$ for all $i=1,\ldots,n$ implies that $\sum_{i=1}^{n}\psi_{i}^{2}\leq\sum_{i=1}^{n}f_{i}^{2}\leq\kappa^{2}$ , it follows that

~{}\mathbb{P}\left(\sum_{i=1}^{n}\psi_{i}^{2}\leq\kappa^{2}\right)\geq\mathbb{P}\left(\bigwedge_{i=1}^{n}|\psi_{i}|\leq f_{i}\right).

(39)

From the reverse union bound (see Appendix B), we have


$\displaystyle\mathbb{P}\left(\bigwedge_{i=1}^{n}\|\psi_{i}\|\leq f_{i}\right)$	$\displaystyle\geq\sum_{i=1}^{n}\mathbb{P}(\|\psi_{i}\|\leq f_{i})-(n-1)$	(40a)
	$\displaystyle\geq\sum_{i=1}^{n}(1-\beta_{i}\delta)-(n-1)$	(40b)
	$\displaystyle=1-\delta\sum_{i=1}^{n}\beta_{i}=1-\delta.$	(40c)

Lastly, combining (39) and (40c) gives (36), which concludes the proof. ∎

We now present two methods for approximating the two-sided chance constraints (38a), one that utilizes a result from [27], and one that utilizes the reverse union bound.

IV-A1 Three-cut Outer Approximation

We state, without proof, the following lemma from [27].

Lemma 1.

Let $\xi\sim\mathcal{N}(\mu,\Sigma)$ be a jointly distributed Gaussian random vector with mean $\mu$ and positive definite covariance matrix $\Sigma$ , and let $\delta\in(0,0.5]$ . Let $LL^{\intercal}=\Sigma$ be the Cholesky decomposition of $\Sigma$ . Let $a,b\in\mathbb{R}$ and $\eta\in\mathbb{R}^{n}$ be decision variables. Then,


$\displaystyle t$	$\displaystyle\geq\\|L^{\intercal}\delta\\|_{2},$	(41a)
$\displaystyle a-\mu^{\intercal}\eta$	$\displaystyle\leq\Phi^{-1}(\delta)t,$	(41b)
$\displaystyle b-\mu^{\intercal}\eta$	$\displaystyle\geq\Phi^{-1}(1-\delta)t,$	(41c)
$\displaystyle a-b$	$\displaystyle\leq 2\Phi^{-1}(\delta/2)t,$	(41d)

is a second order cone (SOC) outer approximation of the constraint

\mathbb{P}(a\leq\eta^{\intercal}\xi\leq b)\geq 1-\delta,

(42)

which, in fact, guarantees

\mathbb{P}(a\leq\eta^{\intercal}\xi\leq b)\geq 1-1.25\delta.

(43)

Using Lemma 1 and Proposition 1, it follows immediately that the constraint (34) is satisfied if the following convex constraints are satisfied, for all $i=1,\ldots,n$ and $k=1,\ldots,N$


$\displaystyle t_{i,k}\geq\\|\Sigma_{Y}^{1/2}(I+\mathcal{B}$	$\displaystyle K)^{\intercal}E_{k}^{\intercal}a_{i}\\|_{2},$	(44a)
$\displaystyle-[f_{i,k}+b_{i}+a_{i}^{\intercal}E_{k}(\mathcal{A}\mu_{0}+\mathcal{B}V)]$	$\displaystyle\leq\Phi^{-1}(\beta_{i}\delta_{k})t_{i,k},$	(44b)
$\displaystyle f_{i,k}-b_{i}-a_{i}^{\intercal}E_{k}(\mathcal{A}\mu_{0}+\mathcal{B}V)$	$\displaystyle\geq\Phi^{-1}(1-\beta_{i}\delta_{k})t_{i,k},$	(44c)
$\displaystyle-f_{i,k}$	$\displaystyle\leq\Phi^{-1}(\beta_{i}\delta_{k}/2)t_{i,k},$	(44d)

for the decision variables $V,K,\mathbf{t}:=[\mathbf{t}_{1},\ldots,\mathbf{t}_{N}]^{\intercal}$ , and $\mathbf{f}:=[\mathbf{f}_{1},\ldots,\mathbf{f}_{N}]$ , where $\mathbf{t}_{k}:=[t_{1,k},\ldots,t_{n,k}]$ and $\mathbf{f}_{k}:=[f_{1,k},\ldots,f_{n,k}]$ .

IV-A2 Reverse Union Bound Approximation

The following proposition gives an alternate approximation of the two-sided chance constraint (38a) using the cumulative distribution function of the normal distribution.

Proposition 2.

Let $\epsilon^{1}_{i,k},\epsilon^{2}_{i,k}>0$ for all $i=1,\ldots,n$ and $k=1,\ldots,N$ . Assume that the convex SOC constraints


	$\displaystyle a_{i}^{\intercal}E_{k}(\mathcal{A}\mu_{0}+\mathcal{B}V)+\\|\Sigma_{Y}^{1/2}(I+\mathcal{B}K)^{\intercal}E_{k}^{\intercal}a_{i}\\|_{2}\Phi^{-1}(\epsilon^{1}_{i,k})$
	$\displaystyle\hskip 170.71652pt\leq f_{i,k}-b_{i},$		(45a)
	$\displaystyle-a_{i}^{\intercal}E_{k}(\mathcal{A}\mu_{0}+\mathcal{B}V)+\\|\Sigma_{Y}^{1/2}(I+\mathcal{B}K)^{\intercal}E_{k}^{\intercal}a_{i}\\|_{2}\Phi^{-1}(\epsilon^{2}_{i,k})$
	$\displaystyle\hskip 170.71652pt\leq f_{i,k}+b_{i},$		(45b)

are satisfied for some $\epsilon^{1}_{i,k}+\epsilon^{2}_{i,k}\geq 2-\beta_{i}\delta_{k}$ , $V$ and $K$ . Then, the chance constraints (38a) are satisfied as well.

Proof.

Equation (38a) can equivalently be written as

\displaystyle\mathbb{P}(-f_{i,k}-b_{i}\leq a_{i}^{\intercal}x_{k}\leq f_{i,k}-b_{i})\geq 1-\beta_{i}\delta_{k},

(46)

or, equivalently, as

\displaystyle\mathbb{P}(a_{i}^{\intercal}x_{k}\leq f_{i,k}-b_{i}\wedge a_{i}^{\intercal}x_{k}\geq-f_{i,k}-b_{i})\geq 1-\beta_{i}\delta_{k}.

(47)

Applying the reverse union bound on (47) yields

	$\displaystyle\mathbb{P}(a_{i}^{\intercal}x_{k}\leq f_{i,k}-b_{i}\wedge a_{i}^{\intercal}x_{k}\geq-f_{i,k}-b_{i})$
	$\displaystyle\geq\mathbb{P}(a_{i}^{\intercal}x_{k}\leq f_{i,k}-b_{i})+\mathbb{P}(a_{i}^{\intercal}x_{k}\geq-f_{i,k}-b_{i})-1.$		(48)

Thus, if the constraint

\mathbb{P}(a_{i}^{\intercal}x_{k}\leq f_{i,k}-b_{i})+\mathbb{P}(a_{i}^{\intercal}x_{k}\geq-f_{i,k}-b_{i})\geq 2-\beta_{i}\delta_{k},

(49)

is satisfied, then inequality (47) holds and the original chance constraints (38a) will be satisfied as well. The constraint in (49) is equivalent to the following decoupled constraints


	$\displaystyle\mathbb{P}(a_{i}^{\intercal}x_{k}\leq f_{i,k}-b_{i})\geq\epsilon^{1}_{i,k},$		(50a)
	$\displaystyle\mathbb{P}(a_{i}^{\intercal}x_{k}\geq-f_{i,k}-b_{i})\geq\epsilon^{2}_{i,k},$		(50b)
	$\displaystyle\epsilon^{2}_{i,k}+\epsilon^{2}_{i,k}\geq 2-\beta_{i}\delta_{k}.$		(50c)

Since $a_{i}^{\intercal}x_{k}$ is a Gaussian random variable with mean $a_{i}^{\intercal}\bar{x}_{k}$ and covariance $a_{i}^{\intercal}E_{k}\Sigma_{X}E_{k}^{\intercal}a_{i}$ , it follows that the probabilities in (50a)-(50b) can be written in terms of the standard normal cumulative distribution function as follows


	$\displaystyle\Phi\left[\frac{f_{i,k}-b_{i}-a_{i}^{\intercal}\bar{x}_{k}}{\sqrt{a_{i}^{\intercal}E_{k}\Sigma_{X}E_{k}^{\intercal}a_{i}}}\right]\geq\epsilon^{1}_{i,k},$		(51a)
	$\displaystyle\Phi\left[\frac{f_{i,k}+b_{i}+a_{i}^{\intercal}\bar{x}_{k}}{\sqrt{a_{i}^{\intercal}E_{k}\Sigma_{X}E_{k}^{\intercal}a_{i}}}\right]\geq\epsilon^{2}_{i,k}.$		(51b)

Finally, substituting the mean dynamics (14) and the corresponding covariance in to (51) yields the desired result. ∎

Remark 8: In the three-cut approximation, the parameters $\mathbf{\beta}:=[\beta_{1},\ldots,\beta_{n}]^{\intercal}$ need to be fixed so that the resulting constraints in (44) are not bilinear in the decision variables. Similarly, in the reverse union bound approximation, the parameters $\epsilon^{1}_{i,k}$ and $\epsilon^{2}_{i,k}$ need to be fixed so that the resulting constraints in (45) are not bilinear in the decision variables.

Remark 9: In (38b), the constraints can be equivalently written as

\|\mathbf{f}_{k}\|_{2}\leq\kappa_{k},~{}~{}k=1,\ldots,N.

(52)

Plugging in the mean dynamics (14) into the definition of $\kappa_{k}$ yields the SOC constraints

\|\mathbf{f}_{k}\|_{2}\leq c^{\intercal}E_{k}(\mathcal{A}\mu_{0}+\mathcal{B}V)+d,\quad k=1,\ldots,N.

(53)

In summary, the relaxed cone chance constraints (34) can be approximated using two-sided chance constraints as in (44) with (53) through the three-cut approximation, or alternatively as in (45) with (53) through the reverse union bound inequality. Since these constraints are convex, the resulting problem is convex and can be solved using standard SDP solvers, similarly to the polyhedral chance constraint case. In the three-cut approximation, there will a total of $4n+1$ constraints per time step, or $(4n+1)N$ total SOC constraints. In the reverse union bound approximation, there will be a total of $2n+1$ constraints per time step, or $(2n+1)N$ total SOC constraints. Although the RUB relaxation is computationally more efficient, the three-cut relaxation is, in general, less conservative. This was confirmed from our numerical examples in Section V.

IV-B Geometric Approximation

We limit the following discussion to the three-dimensional case, which is often the case in practice when enforcing position constraints. However, the results can be generalized to $n$ -dimensional convex cones by using a concentration inequality for $\chi^{2}$ random variables [28]. For simplicity, let $b=0$ in (32), which corresponds to a cone centered at the origin. Letting the state be $x:=[p^{\intercal},\dot{p}^{\intercal}]^{\intercal}$ , where $p\in\mathbb{R}^{3}$ denotes the position, the chance constraints (33) become

\displaystyle\mathbb{P}\left(\bigg{\|}\begin{bmatrix}A_{c}&0\\ 0&0\end{bmatrix}\begin{bmatrix}p_{k}\\ \dot{p}_{k}\end{bmatrix}\bigg{\|}\leq[c_{c}\ 0]^{\intercal}\begin{bmatrix}p_{k}\\ \dot{p}_{k}\end{bmatrix}+d\right)\geq 1-\delta_{k},

(54)

or equivalently,

\displaystyle\mathbb{P}(\|A_{c}p_{k}\|\leq c_{c}^{\intercal}p_{k}+d)\geq 1-\delta_{k},

(55)

where $c_{c}\in\mathbb{R}^{3},A_{c}\in\mathbb{R}^{3\times 3}$ parametrize the cone. In most three-dimensional applications, the matrix $A_{c}$ has two nonzero diagonal elements, and one zero diagonal element. As such, the vector $A_{c}p_{k}$ will have one zero element. Let $H\in\mathbb{R}^{2\times 3}$ be defined such that it extracts the nonzero elements of $A_{c}p_{k}$ . This is needed so as to reduce the dimensionality of the random vector inside the norm, from which we can more easily approximate the chance constraint. It follows that the chance constraints (55) become

\mathbb{P}(\|HA_{c}I_{p}E_{k}X\|\leq c_{c}^{\intercal}I_{p}E_{k}X+d)\geq 1-\delta_{k},

(56)

where $I_{p}:=[I_{3},0_{3}]\in\mathbb{R}^{3\times 6}$ . From a geometric point of view, one can think of the constraints (56) as imposing, at each time step $k$ , that the random vector $\xi_{k}:=HA_{c}p_{k}\in\mathbb{R}^{2}$ lies inside the disk of radius $r_{k}=c_{c}^{\intercal}p_{k}+d$ with probability greater than $1-\delta_{k}$ , However, since $X$ is a stochastic process, it follows that the radius of the disk is uncertain, therefore, and similar to Section IV-A, we relax the chance constraint such that the Gaussian vector $\xi$ lies within the mean radius of the disk $\bar{r}_{k}=c_{c}^{\intercal}I_{p}E_{k}\bar{X}+d$ .

Using this approximation, the chance constraints (33a) become

\mathbb{P}(\|\xi_{k}\|_{2}\leq\bar{r}_{k})\geq 1-\delta_{k}.

(57)

Note that the random variable $\xi_{k}=AE_{k}X$ is Gaussian such that $\xi_{k}\sim\mathcal{N}(\bar{\xi}_{k},\Sigma_{\xi_{k}})$ , with mean $\bar{\xi}_{k}:=HA_{c}I_{p}E_{k}\bar{X}$ and covariance $\Sigma_{\xi_{k}}:=HA_{c}I_{p}E_{k}\Sigma_{X}E_{k}^{\intercal}I_{p}^{\intercal}A_{c}^{\intercal}H^{\intercal}$ . So far, we have turned the convex cone chance constraint (33a) into the chance constraint (57) that requires the probability of a Gaussian random vector being inside a circle of a given radius be greater than $1-\delta_{k}$ . Similar to the methodology in [28], this problem can be analytically solved as follows.

Proposition 3.

Let $\zeta\sim\mathcal{N}(0,\Sigma_{\zeta})$ be a two-dimensional random vector. Then, for $a>0$ ,

\mathbb{P}\big{(}\zeta^{\intercal}\Sigma_{\zeta}^{-1}\zeta\leq a^{2}\big{)}=1-e^{-\frac{1}{2}a^{2}}.

(58)

Proof.

The probability density function (PDF) of $\zeta$ is given by

\mathcal{N}(0,\Sigma_{\zeta})=\frac{1}{2\pi|\det\Sigma_{\zeta}|^{\frac{1}{2}}}e^{-\frac{1}{2}\zeta^{\intercal}\Sigma_{\zeta}^{-1}\zeta}.

(59)

Then, the probability in (58) is given explicitly by

\mathbb{P}(\zeta^{\intercal}\Sigma_{\zeta}^{-1}\zeta\leq a^{2})=\frac{1}{2\pi|\det\Sigma_{\zeta}|^{\frac{1}{2}}}\int_{\Omega_{\zeta}}e^{-\frac{1}{2}\zeta^{\intercal}\Sigma_{\zeta}^{-1}\zeta}\,\mathrm{d}\zeta,

(60)

where $\Omega_{\zeta}:=\{\zeta:\zeta^{\intercal}\Sigma_{\zeta}^{-1}\zeta\leq a^{2}\}$ . Changing coordinates such that $\nu:=\Sigma_{\zeta}^{-\frac{1}{2}}\zeta=(\rho\cos\phi,\rho\sin\phi)$ so that $\mathrm{d}\nu=|\det\Sigma_{\zeta}|^{-\frac{1}{2}}\,\mathrm{d}\zeta$ , note that the sets $\{\zeta^{\intercal}\Sigma_{\zeta}^{-1}\zeta\leq a^{2}\}$ and $\{\|\nu\|_{2}\leq a\}$ are equivalent. Thus, the integral in (60) becomes

\mathbb{P}(\zeta^{\intercal}\Sigma_{\zeta}^{-1}\zeta\leq a^{2})=\mathbb{P}(\|\nu\|_{2}\leq a)=\frac{1}{2\pi}\int_{\Omega_{\nu}}e^{-\frac{1}{2}\nu^{\intercal}\nu}\,\mathrm{d}\nu,

(61)

where $\Omega_{\nu}:=\{\nu:\|\nu\|_{2}\leq a\}$ . The last integral is straightforward to evaluate in two dimensions, namely,

~{}\mathbb{P}(\|\nu\|_{2}\leq a)=\frac{1}{2\pi}\int_{0}^{2\pi}\int_{0}^{a}e^{-\frac{1}{2}\rho^{2}}\rho\,\mathrm{d}\rho\,\mathrm{d}\phi=1-e^{-\frac{1}{2}a^{2}},

(62)

which yields the desired result. ∎

Lemma 2.

Let $\zeta\sim\mathcal{N}(0,\Sigma_{\zeta})$ be a two-dimensional random vector, let $\sigma_{\zeta}^{2}=\lambda_{\mathrm{max}}(\Sigma_{\zeta})$ , and let $r>0$ . Then,

~{}\mathbb{P}(\|\zeta\|_{2}\leq r)\geq 1-e^{-r^{2}/2\sigma_{\zeta}^{2}}.

(63)

Proof.

Since the covariance matrix is positive definite, we can decompose it as $\Sigma_{\zeta}=PDP^{\intercal}$ where $D$ is a diagonal matrix containing the eigenvalues $\lambda_{i}$ of $\Sigma_{\zeta}$ and $P$ is an orthogonal matrix. Since $\sigma_{\zeta}^{2}=\max_{i}\lambda_{i}$ , it follows that

D^{-1}=\frac{1}{\sigma_{\zeta}^{2}}\text{diag}(\sigma_{\zeta}^{2}/\lambda_{i})\geq\frac{1}{\sigma_{\zeta}^{2}}I.

(64)

From the previous expression, it follows that

\zeta^{\intercal}\Sigma_{\zeta}^{-1}\zeta=\zeta^{\intercal}PD^{-1}P^{\intercal}\zeta\geq\frac{1}{\sigma_{\zeta}^{2}}\zeta^{\intercal}PP^{\intercal}\zeta=\frac{1}{\sigma_{\zeta}^{2}}\|\zeta\|_{2}^{2}.

(65)

Rearranging the previous inequality gives $\|\zeta\|_{2}^{2}/\sigma_{\zeta}^{2}\leq\zeta^{\intercal}\Sigma_{\zeta}^{-1}\zeta$ , and using (58), it follows that

\mathbb{P}(\|\zeta\|_{2}^{2}\leq\sigma_{\zeta}^{2}a^{2})\geq\mathbb{P}(\zeta^{\intercal}\Sigma_{\zeta}^{-1}\zeta\leq a^{2})=1-e^{-\frac{1}{2}a^{2}}.

(66)

Setting $r^{2}=\sigma_{\zeta}^{2}a^{2}$ achieves the desired result. Geometrically, the level sets $\{\zeta^{\intercal}\Sigma_{\zeta}^{-1}\zeta=a^{2}\}$ define the contours of ellipses having probability $1-e^{-a^{2}/2}$ and the level sets $\{\|\zeta\|_{2}^{2}=r^{2}\}$ are the smallest circles that contain these ellipses. ∎

Theorem 2.

Let $\xi\sim\mathcal{N}(\bar{\xi},\Sigma_{\xi})$ be a two-dimensional random vector, let $\sigma_{\xi}^{2}=\lambda_{\mathrm{max}}(\Sigma_{\xi})$ , and let $r>0$ . Then,

~{}\|\bar{\xi}\|_{2}+\sigma_{\xi}\sqrt{2\log\frac{1}{\delta}}\leq r\Rightarrow\mathbb{P}(\|\xi\|_{2}\leq r)\geq 1-\delta.

(67)

Proof.

First, note that for $\|\xi\|_{2}\leq r$ , the following implications hold


$\displaystyle\\|\bar{\xi}\\|_{2}+$	$\displaystyle\sigma_{\xi}\sqrt{2\log\frac{1}{\delta}}\leq r\Rightarrow\sigma_{\xi}\sqrt{2\log\frac{1}{\delta}}\leq r-\\|\bar{\xi}\\|$	(68a)
	$\displaystyle\Rightarrow 2\sigma_{\xi}^{2}\log\frac{1}{\delta}\leq(r-\\|\bar{\xi}\\|_{2})^{2}$	(68b)
	$\displaystyle\Rightarrow 2\sigma_{\xi}^{2}\log\delta\geq-(r-\\|\bar{\xi}\\|_{2})^{2}$	(68c)
	$\displaystyle\Rightarrow\log\delta\geq-\frac{(r-\\|\bar{\xi}\\|_{2})^{2}}{2\sigma_{\xi}^{2}}$	(68d)
	$\displaystyle\Rightarrow\delta\geq\exp\bigg{(}-\frac{(r-\\|\bar{\xi}\\|_{2})^{2}}{2\sigma_{\xi}^{2}}\bigg{)}$	(68e)
	$\displaystyle\Rightarrow 1-\delta\leq 1-\exp\bigg{(}-\frac{(r-\\|\bar{\xi}\\|_{2})^{2}}{2\sigma_{\xi}^{2}}\bigg{)}.$	(68f)

Since $\{\xi:\|\bar{\xi}\|_{2}+\|\tilde{\xi}\|_{2}\leq r\}\subseteq\{\xi:\|\xi\|_{2}\leq r\}$ , where $\tilde{\xi}:=\xi-\bar{\xi}$ , it follows that

\mathbb{P}(\|\xi\|_{2}\leq r)\geq\mathbb{P}(\|\bar{\xi}\|_{2}+\|\tilde{\xi}\|_{2}\leq r)=\mathbb{P}(\|\tilde{\xi}\|_{2}\leq r-\|\bar{\xi}\|_{2}).

(69)

Since $\tilde{\xi}$ is a zero-mean Gaussian vector, applying Lemma 2 gives

\mathbb{P}(\|\tilde{\xi}\|_{2}\leq r-\|\bar{\xi}\|_{2})\geq 1-\exp\bigg{(}-\frac{(r-\|\bar{\xi}\|_{2})^{2}}{2\sigma_{\xi}^{2}}\bigg{)}.\\

(70)

Finally, by (68) and (69), we obtain the desired result. ∎

Using Theorem 2, we can now satisfy (57) by enforcing

\sigma_{\xi_{k}}\sqrt{2\log\frac{1}{\delta_{k}}}\leq\bar{r}_{k}-\|\bar{\xi}_{k}\|=:\bar{R}_{k}.

(71)

Using $\Sigma_{X}=(I+\mathcal{B}K)\Sigma_{Y}(I+\mathcal{B}K)^{\intercal}$ and noting that $\sigma_{\xi_{k}}^{2}=\lambda_{\textrm{max}}(\Sigma_{\xi})$ , we obtain

\sigma_{\xi_{k}}^{2}=\|\Sigma_{Y}^{1/2}(I+\mathcal{B}K)^{\intercal}E_{k}^{\intercal}I_{p}^{\intercal}A_{c}^{\intercal}H^{\intercal}\|_{2}^{2}.

(72)

In summary, the cone chance constraints (33a) become

\displaystyle\!\sqrt{2\log\frac{1}{\delta_{k}}}\|\Sigma_{Y}^{1/2}(I+\mathcal{B}K)^{\intercal}E_{k}^{\intercal}I_{p}^{\intercal}A_{c}^{\intercal}H^{\intercal}\|_{2}\leq\bar{R}_{k},

(73)

for $k=1,\ldots,N$ .

V Spacecraft Rendezvous Example

V-A IRA-CS with Polytopic Chance Constraints

In this section, we implement the previous theory of CS with optimal risk allocation to the problem of spacecraft proximity operations in orbit. We consider the problem where one of the spacecraft, called the Deputy, approaches and docks with the second spacecraft, called the Chief, such that in the process, the Deputy remains within the line-of-sight (LOS) of the Chief, defined initially to be the polytopic region shown in Figure 1.

Refer to caption — Figure 1: Feasible state space region for spacecraft rendezvous problem.

It is assumed that the two spacecraft are in the LVLH frame, that is, a rotating reference frame where the $z$ axis is oriented in the direction of the center of the Earth, the $y$ axis negative to the orbit normal, and the $x$ axis to complete the right hand rule. Moreover, assuming that the Chief is in a circular orbit (at an altitude $h=800$ km), the relative dynamics of the motion between the two spacecraft are given by the Clohessy-Wiltshire-Hill Equations [29],


$\displaystyle\ddot{x}$	$\displaystyle=3\omega^{2}x+2\omega\dot{y}+F_{x}/m_{d},$	(74a)
$\displaystyle\ddot{y}$	$\displaystyle=-2\omega\dot{x}+F_{y}/m_{d},$	(74b)
$\displaystyle\ddot{z}$	$\displaystyle=-\omega^{2}z+F_{z}/m_{d},$	(74c)

where $m_{d}=300$ kg is the mass of the Deputy, $\omega=\sqrt{\mu/R_{0}^{3}}$ is the orbital frequency, and $F\coloneqq[F_{x},F_{y},F_{z}]^{\intercal}$ represents the thrust input components to the spacecraft. It is assumed that the thrust is generated by a chemical propulsion system with PWM (pulse-width-modulation) able to implement continuous thrust profiles from impulsive forces.

The equations of motion (74) are written in a relative coordinate system, where the Chief is located at the origin, and $x,y,z$ represent the position of the Deputy with respect to the Chief. Note that the $z$ dynamics are decoupled from the $x-y$ dynamics; furthermore, the $z$ dynamics are globally asymptotically stable, so in theory we only need to control the planar dynamics. In Figure 1 the blue area represents the planar region. To write the system in state space form, let $x\coloneqq[p_{x},p_{y},p_{z},\dot{p}_{x},\dot{p}_{y},\dot{p}_{z}]^{\intercal}\in\mathbb{R}^{6}$ to obtain the LTI system $\dot{x}=Ax+Bu$ , where

~{}A=\begin{bmatrix}0&0&0&1&0&0\\ 0&0&0&0&1&0\\ 0&0&0&0&0&1\\ 3\omega^{2}&0&0&0&2\omega&0\\ 0&0&0&-2\omega&0&0\\ 0&0&-\omega^{2}&0&0&0\end{bmatrix},\ B=m_{c}^{-1}[0_{3},I_{3}]^{\intercal},

(75)

and $u\coloneqq[F_{x},F_{y},F_{z}]^{\intercal}\in\mathbb{R}^{3}$ . To discretize the system, we divide the time interval into $N=15$ steps, with a time interval $\Delta t=4$ sec. Assuming a zero-order hold (ZOH) on the control and adding noise that captures any modeling and discretization errors, as well as other environmental disturbances, yields the discrete system

x_{k+1}=A_{d}x_{k}+B_{d}u_{k}+Gw_{k},

(76)

where $A_{d}=e^{A\Delta t}$ , $B_{d}=\int_{0}^{\Delta t}e^{A\tau}B\ \mathrm{d}\tau$ . We choose the associated noise characteristics as $G=\text{diag}(10^{-4},10^{-4},5\times 10^{-8},5\times 10^{-8})$ [30]. We assume that the initial state mean and covariance are $\mu_{0}=[90,-120,90,0,0,0]^{\intercal}$ and $\Sigma_{0}=\text{diag}(10,10,10,1,1,1)$ , respectively. We wish to steer the distribution from the above initial state to the final mean $\mu_{f}=0$ with final covariance $\Sigma_{f}=\frac{1}{4}\Sigma_{0}$ , while minimizing the cost function (4) with weight matrices $Q=\text{diag}(10,10,10,1,1,1)$ and $R=10^{3}I_{3}$ . We impose the joint probability of failure over the whole horizon to be $\Delta=0.03$ , which implies that the probability of violating any state constraint over the whole horizon is less than 3%. The control inputs are bounded as $\|u_{k}\|_{\infty}\leq 30$ N, which corresponds to a maximum acceleration of 10 cm/s². Note that these bounds are hard constraints as opposed to (soft) chance constraints. To implement this input hard constraint within the CS framework, the algorithm in [15] was used (see also Appendix C). It should be noted here that since saturation of the input may lead to non-Gaussian state evolution, the chance constraint inequality (23) may not hold anymore. For our purposes though, the formulated SOC constraints work well even for the non-Gaussian case. This may lead to somewhat more conservative results, but for our problem, the difference turned out to be negligible.

Lastly, in the iterative risk allocation algorithm, we use a scaling parameter $\rho_{(i)}=(0.7)(0.98)^{i}$ in Line 10 of the algorithm, where $i$ represents the current iteration. The SDP in Problem 2 was implemented in MATLAB using YALMIP[31] along with MOSEK[32] to solve the relevant optimization problems.

Figures 2 and 3 show the trajectories with optimal risk allocation, and Figure 3 shows the two-dimensional planar motion. Figure 4 compares the terminal trajectories of CS with a uniform risk allocation with the proposed method. The two solutions look similar and both satisfy the terminal constraints on the mean and the covariance. However, due to the relaxation $\Sigma_{N}\leq\Sigma_{f}$ , the uniform risk allocation scheme leads to more conservative solutions, as shown in Figure 4. The volume of the final covariance ellipsoid, $V_{N}\propto\log\det\Sigma_{N}$ is smaller for the uniform allocation solution compared to the optimal allocation solution (see Table I). In fact, we see that a consequence of optimal risk allocation is that it maximizes the final covariance given all the constraints, while still being bounded by $\Sigma_{f}$ .

Figure 5 shows the state trajectories and the optimal controls for the polyhedral chance constraints. The control is almost linear but has a large variance in the first 10 time steps, where it may saturate due to the disturbances. Figure 8 shows the a priori allocation of risk, as well as the true risk $\bar{\delta}$ once the optimization is completed, where $\delta_{r}$ corresponds to the risk allocated for the right boundary and $\delta_{u}$ for the risk allocated for the top boundary. Notice that in Figure 8(a) the true risk exposure is much lower than the allocated risk, which confirms the conclusion that the solutions for the uniform allocation case can be overly conservative. Comparing to Figure 8(b), we see a close correspondence between the allocated risk and the true risk exposure over the whole horizon for the optimal risk allocation case. It should be noted that although the true risk is still slightly less than the allocated risk, the error between the two is much smaller when compared to that of the uniform risk allocation strategy.

The iterative risk allocation algorithm is robust in the sense that the algorithm will assign risk proportionately to how close the solution trajectories are to the boundaries of the state space. Since solutions are close to the right and top boundaries of the allowable LOS region for most of the horizon, the optimal allocation weighs the respective risks more than those of the left and bottom boundaries. Thus, IRA purposefully steers the trajectories away from the boundaries proportionally to the amount of risk that is allocated to violating those respective boundaries. Table I shows the true joint probability of failure, defined as

~{}\bar{\Delta}\coloneqq 1-\mathbb{P}\Bigg{[}\bigwedge_{k=1}^{N}\bigwedge_{j=1}^{M}\alpha_{j}^{\intercal}E_{k}X^{*}\leq\beta_{j}\Bigg{]}.

(77)

It is clear that the uniform risk allocation does not even come close to the desired design of $\Delta=0.03$ , while the IRA gives a true probability of failure very close to the desired one.

TABLE I: Comparison of total true risk and terminal volume between uniform and optimal allocations.

-	Uniform	IRA Poly	IRA TC	IRA RUB
$\bar{\Delta}$	0.0123	0.02998	0.029990	0.029994
$V_{N}$	0.5546	0.6279	3.6818	3.7038
-	IRA GEO	CS-SMPC [17]	SMPC [33]
$\bar{\Delta}$	0.029979	0.01128	0.00105
$V_{N}$	4.0909	-	-

Finally, we looked at the optimal cost function over each IRA iteration, as in Figure 9. The convergence criterion set in this example is $\epsilon=10^{-5}$ , or when all of the constraints are active or inactive, which can be proved in [26] to be a sufficient condition for optimality for Problem 3. We see that indeed (29) holds, and the optimization resulted even in a slight decrease of the objective function. Thus, the iterative risk allocation algorithm optimizes the risk allocation at each time step without increasing the cost.

V-B IRA-CS with Cone Chance Constraints

For this example, the following representation of a cone was used

~{}\mathcal{X}^{c}=\{(x,y,z):\|A\bar{R}_{x}(\psi)x\|_{2}\leq c^{\intercal}\bar{R}_{x}(\psi)x+d\},

(78)

where $\bar{R}_{x}(\psi):=\textrm{blkdiag}(R_{x}(\psi),R_{x}(\psi))$ and $R_{x}(\psi)\in\mathrm{SO(3)}$ is the standard 3D rotation matrix. The angle of rotation is $\psi=\tan^{-1}(\bar{z}_{0}/\bar{y}_{0})$ , which corresponds to a body-mounted sensor on the Chief that is angled to the relative position vector of the Deputy. Additionally, $A=\textrm{diag}(1,0,1,0,0,0),\ c=[0,\lambda,0]^{\intercal}$ , and $\lambda=\tan(\theta)$ , where $\theta=15^{\circ}$ is the cone half-angle. Lastly, $d=10$ , which corresponds to an offset of 10 meters from the origin. For the geometric approximation, we set $A_{c}=\textrm{diag}(1,0,1),h_{1}=[1,0,0]^{\intercal},h_{2}=[0,0,1]^{\intercal}$ , and $c_{c}=[0,\lambda,0]^{\intercal}$ . The constant choice of parameters $\beta_{i}=1/n$ for all $i=1,\ldots,n$ was used across all constraints in the three-cut approximation and $\epsilon_{1}=\epsilon_{2}=1-\beta_{i}\delta_{k}/2$ was used for the reverse union bound approximation. Lastly, the initial state was changed to $\mu_{0}=[10,120,90,0,0,0]^{\intercal}$ for this simulation.

It should be noted that a rotated cone about the $i$ th axis can be put in the form of a standard cone as in (32) via the transformation

\tilde{A}:=A\bar{R}_{i}(\psi),\quad\tilde{c}:=\bar{R}^{\intercal}_{i}(\psi)c.

(79)

Thus, it is possible to make successive rotations of a cone by adjusting the cone parameters $A$ and $c$ . In the context of the given parametrization, the cone becomes the set

\bigg{\|}\begin{bmatrix}p_{x}\\ p_{y}\sin\psi+p_{z}\cos\psi\end{bmatrix}\bigg{\|}_{2}\leq\gamma(p_{y}\cos\psi-p_{z}\sin\psi)+d.

(80)

Figure 9 compares the results of the three approximations, with the geometric one being the best and the three-cut approximation being less conservative than the RUB approximation, as expected. Figure 12 shows the optimal trajectories in the three-dimensional space and in the projection on the $x$ - $y$ plane, respectively. It should be noted that for both the three-cut and RUB approximations of the cone constraint, and since we approximated the quadratic constraints as $\mathcal{O}(n)$ SOC constraints at each time step, the IRA algorithm needs to be adjusted as follows. In Line 5 of Algorithm 1, a constraint is active at time step $k$ if any of the constraints in (38a) are active. Similarly, when tightening the constraints in Line 10, the maximum true risk $\bar{\delta}_{k}:=\max_{j}\bar{\delta}_{k}^{j}$ is used. This is not needed for the geometric approximation because it approximates each cone chance constraint as a single convex constraint for each $k$ , so the standard IRA algorithm is applicable.

From Table I, the geometric approximation actually has the highest terminal covariance of all four IRA methods discussed. Other than that, the two optimal inputs and trajectories are very similar from Figure 8, and both successfully steer the distribution of states to the terminal distribution, while satisfying the cone constraints.

V-C Comparison with MPC-based Controllers

In addition to comparing the presented methods with each other, it is also worthwhile to compare these methods to MPC-based methods in the context of the spacecraft rendezvous problem. Although there exists a significant literature on the use of MPC-based approaches for satellite rendezvous and proximity operations in space [34, 35, 36, 37, 38], most of these results assume deterministic dynamics and do not handle directly chance constraints. To this end, we applied two recent stochastic MPC (SMPC) formulations, outlined in [17] and [33], and compared them to the present formulation. Note that, with the exception of [17] and [33], most SMPC methods [39, 40, 34] assume bounded disturbances and/or chance constraints on the input, while in the present case, we assume more general, unbounded disturbances with hard constraints on the input, making the problem much harder to solve as a result.

Figure 15 shows the result from the MPC-based methods in [17] and [33]. In [17], the authors iteratively solve a covariance steering problem over a receding horizon. There are two terminal constraints at every time step; the first is on the mean state to be inside a maximally positive invariant (MPI) set defined by $(A+B\tilde{K})\mu\in\mathcal{X}_{f}^{\mu}$ , where $\tilde{K}$ is the associated gain that satisfies $\Sigma=(A+B\tilde{K})\Sigma(A+B\tilde{K})^{\intercal}+DD^{\intercal}$ , and $\Sigma$ is an assignable state covariance [41]. The second, and similar to this work, is a constraint on the terminal covariance to be less than or equal to $\Sigma_{f}$ , where $\Sigma_{f}$ is the aforementioned assignable covariance. Similarly, in [33] there are again two terminal constraints on the mean and the covariance, but in this case, the MPI set is defined as $(A+BK_{\textrm{LQR}})\mu\in\mathcal{X}_{f}^{\mu}$ , where $K_{\textrm{LQR}}$ is the gain computed as the solution of an LQ problem for the nominal system, and $\Sigma_{f}$ is chosen to solve the Lyapunov equation $\Sigma_{f}=(A+BK_{\textrm{LQR}})\Sigma_{f}(A+BK_{\textrm{LQR}})^{\intercal}+DD^{\intercal}$ .

In [33], the algorithm was not able to solve the rendezvous problem with the given parameters for a control bound of $u_{\textrm{max}}=30$ N, as this was not enough control authority to reach the MPI set at the end of the horizon ( $N=10,\Delta t=2$ ). The MPC problems became feasible only when the bounds were relaxed to more than five times that of the current work. Additionally, in both SMPC formulations, the designer is not allowed to freely choose the terminal covariance, since in order for the problem to be feasible, the terminal covariance must satisfy certain constraints. Lastly, although both MPC controllers successfully steer the system to the origin, there are no guarantees of recursive feasibility for the closed loop system and the solutions are more sub-optimal compared to that of the current work, as there is no optimization over the risks since they are uniformly allocated. As a result, the true risk in these MPC methods is much lower than the design goal, as shown in Table I. Further, there is no current work to date that can incorporate MPC-based methods for conical state spaces as was proposed here. As such, one potential future application of the proposed CS-IRA algorithm is in the context of SMPC, which would lead to less conservative and more optimal trajectory designs, without having to uniformly allocate risk or use a polyhedral approximation of the LOS cone.

VI Conclusion

In this paper, we have incorporated an iterative risk allocation (IRA) strategy to optimize the probability of violating the state constraints at every time step within the covariance steering problem of a linear stochastic system subject to chance constraints. For the covariance steering problem, we showed that employing IRA not only leads to less conservative solutions that are more practical, but also tends to maximize the final covariance. Additionally, the use of IRA in the context of CS with chance constraints results in optimal solutions that have a true risk much closer to the intended design requirements, compared to the use of a uniform risk allocation. We also implemented quadratic chance constraints in the form of convex cones, which are more accurate and natural for many engineering applications. Using two different relaxation methods these quadratic chance constraints can be made convex and the IRA algorithm can be used to optimize the risk. Lastly, we also propose a geometric approximation of the cone chance constraints, which is valid when the constraint space is three dimensional, as is often the case when constraining the position of a vehicle, and which is less conservative than either of the two two-sided approximations. All proposed relaxations result in convex programs, where the two-stage IRA algorithm is applicable.

VII Acknowledgment

This work has been supported by NASA University Leadership Initiative award 80NSSC20M0163 and ONR award N00014-18-1-2828. The first author also acknowledges support from the Georgia Tech School of Aerospace Engineering Graduate Fellows Program. The article solely reflects the opinions and conclusions of its authors and not any NASA entity.

References

[1] P. Li, M. Wendt, and G. Wozny, “A probabilistically constrained model predictive controller,” Automatica, vol. 38, pp. 1171–1176, 2002.
[2] A. F. Hotz and R. E. Skelton, “A covariance control theory,” in 24th IEEE Conference on Decision and Control, Fort Lauderdale, FL, Dec 11–13, 1985, pp. 552–557.
[3] A. Hotz and R. E. Skelton, “Covariance control theory,” International Journal of Control, vol. 46, no. 1, pp. 13–32, 1987.
[4] E. Bakolas, “Optimal covariance control for discrete-time stochastic linear systems subject to constraints,” in 55th IEEE Conference on Decision and Control, Las Vegas, NV, Dec 12–14, 2016, pp. 1153–1158.
[5] ——, “Finite-horizon covariance control for discrete-time stochastic linear systems subject to input constraints,” Automatica, vol. 91, pp. 61–68, 2018.
[6] A. Halder and E. D. B. Wendel, “Finite horizon linear quadratic gaussian density regulator with wasserstein terminal cost,” in American Control Conference, Boston, MA, July 6–8, 2016, pp. 7249–7254.
[7] Y. Chen, T. T. Georgiou, and M. Pavon, “Optimal steering of a linear stochastic system to a final probability distribution – Part I,” IEEE Transactions on Automatic Control, vol. 61, no. 5, pp. 1158–1169, 2016.
[8] M. Goldshtein and P. Tsiotras, “Finite-horizon covariance control of linear time-varying systems,” in 56th IEEE Conference on Decision and Control, Melbourne, Australia, Dec 12–15 2017, pp. 3606–3611.
[9] E. Bakolas, “Finite-horizon separation-based covariance control for discrete-time stochastic linear systems,” in 57th IEEE Conference on Decision and Control, Miami Beach, FL, Dec 17–19, 2018, pp. 3299–3304.
[10] K. Okamoto and P. Tsiotras, “Optimal stochastic vehicle path planning using covariance steering,” IEEE Robotics and Automation Letters, vol. 4, no. 3, pp. 2276–2281, 2019.
[11] Y. Chen, T. T. Georgiou, and M. Pavon, “Optimal steering of a linear stochastic system to a final probability distribution – Part II,” IEEE Transactions on Automatic Control, vol. 61, no. 5, pp. 1170–1180, 2016.
[12] ——, “Optimal steering of a linear stochastic system to a final probability distribution – Part III,” IEEE Transactions on Automatic Control, vol. 63, no. 9, pp. 3112–3118, 2018.
[13] Y. Chen, T. T. Georgiou, and A. Tannenbaum, “Vector-valued optimal mass transport,” 2017, arXiv:1611.09946.
[14] K. Okamoto, M. Goldshtein, and P. Tsiotras, “Optimal covariance control for stochastic systems under chance constraints,” IEEE Control System Letters, vol. 2, pp. 266–271, 2018.
[15] K. Okamoto and P. Tsiotras, “Input hard constrained optimal covariance steering,” in 58th IEEE Conference on Decision and Control, Nice, France, Dec 11–13, 2019, pp. 3497–3502.
[16] E. Bakolas, “Covariance control for discrete-time stochastic linear systems with incomplete state information,” in American Control Conference, Seattle, WA, 2017, pp. 432–437.
[17] K. Okamoto and P. Tsiotras, “Stochastic model predictive control for constrained linear systems using optimal covariance steering,” 2019, arXiv:1905.13296.
[18] J. Ridderhof, K. Okamoto, and P. Tsiotras, “Nonlinear uncertainty control with iterative covariance steering,” in 58th IEEE Conference on Decision and Control, Nice, France, Dec 11–13 2019, pp. 3484–3490.
[19] E. Bakolas and A. Tsolovikos, “Greedy finite-horizon covariance steering for discrete-time stochastic nonlinear systems based on the unscented transform,” 2020, arXiv:2003.03679.
[20] Z. Yi, Z. Cao, E. Theodorou, and Y. Chen, “Nonlinear covariance control via differential dynamic programming,” 2019, arXiv:1911.09283.
[21] D. H. van Hessem, “Stochastic inequality constrained closed loop model predictive control with application to chemical process operation,” Ph.D. dissertation, Delft University of Technology, 2004.
[22] L. Blackmore, “A probabilistic particle control approach to optimal, robust predictive control,” in AIAA Guidance, Navigation, Control Conference, Keystone, CO, Aug 21–24, 2006, pp. 1–15.
[23] J. Pilipovsky and P. Tsiotras, “Chance-constrained optimal covariance with iterative risk allocation,” in American Control Conference, New Orleans, LA, May 26–28, 2021, submitted.
[24] A. Prékopa, “Boole-Bonferroni inequalities and linear programming,” Operations Research, vol. 36, no. 1, pp. 145–162, 1988.
[25] L. Blackmore, H. X. Li, and B. C. Williams, “A probabilistic approach to optimal robust path planning with obstacles,” in American Control Conference, Minneapolis, MN, June 14–16, 2006, pp. 1–7.
[26] M. Ono and B. C. Williams, “Iterative risk allocation: A new approach to robust model predictive control with a joint chance constraint,” in 47th IEEE Conference on Decision and Control, Cancun, Mexico, Dec 8–11, 2008, pp. 3427–3432.
[27] M. Lubin, D. Bienstock, and J. Vielma, “Two-sided linear chance constraints and extensions,” 2016, arXiv:1507.01995.
[28] J. Ridderhof, J. Pilipovsky, and P. Tsiotras, “Chance-constrained covariance control for low-thrust minimum-fuel trajectory optimization,” in AAS/AIAA Astrodynamics Specialist Conference, Lake Tahoe, CA, Aug 9–13 2020.
[29] W. Weisel, Spaceflight Dynamics. McGraw-Hill Book Co, 1989.
[30] A. Vinod and M. Oishi, “Affine controller synthesis for stochastic reachability via difference of convex programming,” in 58th IEEE Conference on Decision and Control, Nice, France, 2019, pp. 7273–7280.
[31] J. Lofberg, “YALMIP: A toolbox for modeling and optimization in matlab,” in IEEE International Symposium on Computer Aided Control Systems Design, Taipei, Taiwan, 2004, pp. 284–289.
[32] MOSEK ApS, The MOSEK Optimization Toolbox for MATLAB Manual. Version 8.1., 2017. [Online]. Available: http://docs.mosek.com.
[33] M. Farina, L. Giulioni, L. Magni, and R. Scattolini, “A probabilistic approach to model predictive control,” in 52nd IEEE Conference on Decision and Control, Florence, Italy, Dec 10–13 2013, pp. 7734–7739.
[34] M. Mammarella, E. Capello, M. Lorenzen, F. Dabbene, and F. Allgöower, “A general sampling-based SMPC approach to spacecraft proximity operations,” in 56th IEEE Conference on Decision and Control, Melbourne, Australia, 2017, pp. 4521–4526.
[35] A. Weiss, M. Baldwin, R. S. Erwin, and I. Kolmanovsky, “Model predictive control for spacecraft rendezvous and docking: Strategies for handling constraints and case studies,” IEEE Transactions on Control Systems Technology, vol. 23, no. 4, pp. 1638–1647, 2015.
[36] D. C. S., H. Park, and I. Kolmanovsky, “Model predictive control approach for guidance of spacecraft rendezvous and proximity maneuvering,” International Journal of Robust and Nonlinear Control, vol. 22, no. 12, pp. 1398–1427, 2012.
[37] M. Leomanni, E. Rogers, and S. B. Gabriel, “Explicit model predictive control approach for low-thrust spacecraft proximity operations,” Journal of Guidance, Control, and Dynamics, vol. 37, no. 6, pp. 1780–1790, 2014.
[38] H. Park, C. Zagaris, J. Vigili-Llop, R. Zappulla, I. Kolmanovsky, and M. Romano, “Analysis and experimentation of model predictive control for spacecraft rendezvous and proximity operations with multiple obstacle avoidance,” in AIAA/AAS Astrodynamics Specialist Conference, Long Beach, CA, 2016, pp. 4521–4526.
[39] D. Q. Mayne, M. M. Seron, and S. V. Raković, “Robust model predictive control of constrained linear systems with bounded disturbances,” Automatica, vol. 41, no. 2, pp. 219–224, 2005.
[40] M. Mammarella, E. Capello, H. Park, G. Guglieri, and M. Romano, “Tube-based robust model predictive control for spacecraft proximity operations in the presence of persistent disturbance,” Aerospace Science and Technology, vol. 77, pp. 585–594, 2018.
[41] E. Collins and R. Skelton, “A theory of state covariance assignment for discrete systems,” IEEE Transactions on Automatic Control, vol. 32, no. 1, pp. 35–41, 1987.
[42] M. J. Wainwright, High-Dimensional Statistics: A Non-Asymptotic Viewpoint. Cambridge University Press, 2019.
[43] A. W. Marshall and I. Olkin, “Multivariate chebyshev inequalities,” The Annals of Mathematical Statistics, pp. 1001–1014, 1960.

A. Cone Chance Constraint Relaxation

Lemma 3.

The quadratic chance constraint

\mathbb{P}(\|Ax+b\|_{2}\leq c^{\intercal}\mu+d)\geq 1-\delta,

(A.1)

where $x\sim\mathcal{N}(\mu,\Sigma)$ is a relaxation of the cone chance constraint

\!\mathbb{P}(\|Ax+b\|_{2}\leq c^{\intercal}x+d)\geq 1-\delta.

(A.2)

Proof.

Since $x\sim\mathcal{N}(\mu,\Sigma)$ it follows that $\xi:=\|Ax+b\|_{2}$ follows a non-central $\chi^{2}$ distribution with probability density function $f_{\xi}(x)$ [42]. Let $\eta:=c^{\intercal}x+d$ and notice that $\eta\sim\mathcal{N}(c^{\intercal}\mu+d,c^{\intercal}\Sigma c)$ . The chance constraint (A.2) then takes the form $\mathbb{P}(\xi\leq\eta)\geq 1-\delta$ . The probability that one random variable is less than or equal another random variable is given by

\mathbb{P}(\xi\leq\eta)=\int_{-\infty}^{\infty}\int_{-\infty}^{y}f_{\xi,\eta}(x,y)\,\mathrm{d}x\,\mathrm{d}y,

(A.3)

where $f_{\xi,\eta}(x,y)$ is the joint probability distribution function of the random variables $\xi$ and $\eta$ . Next, write $\eta=c^{\intercal}\mu+d+z\sqrt{c^{\intercal}\Sigma c}$ , where $z\sim\mathcal{N}(0,1)$ with probability density $f_{z}(y)$ and let $\bar{\eta}=c^{\intercal}\mu+d$ . The inner integral in (A.3) then becomes

	$\displaystyle\int_{-\infty}^{y}$	$\displaystyle f_{\xi,\eta}(x,y)\,\mathrm{d}x\,=\int_{-\infty}^{\bar{\eta}+y\sqrt{c^{\intercal}\Sigma c}}f_{\xi,z}(x,y)\,\mathrm{d}x$
		$\displaystyle=\int_{-\infty}^{\bar{\eta}}f_{\xi,z}(x,y)\,\mathrm{d}x+\int_{\bar{\eta}}^{\bar{\eta}+y\sqrt{c^{\intercal}\Sigma c}}f_{\xi,z}(x,y)\,\mathrm{d}x.$

It follows that

		$\displaystyle\mathbb{P}(\xi\leq\eta)=\int_{-\infty}^{\infty}\int_{-\infty}^{\bar{\eta}}f_{\xi,z}(x,y)\,\mathrm{d}x\,\mathrm{d}y$
		$\displaystyle\qquad\qquad\qquad+\int_{-\infty}^{\infty}\int_{\bar{\eta}}^{\bar{\eta}+y\sqrt{c^{\intercal}\Sigma c}}f_{\xi,z}(x,y)\,\mathrm{d}x\,\mathrm{d}y$
		$\displaystyle\ \quad\qquad\qquad\ \ \geq\int_{-\infty}^{\infty}\int_{-\infty}^{\bar{\eta}}f_{\xi,z}(x,y)\,\mathrm{d}x\,\mathrm{d}y.$		(A.4)

Noticing that

\int_{-\infty}^{\infty}f_{\xi,z}(x,y)\,\mathrm{d}y=f_{\xi}(x),

the last expression in (Proof.) implies that

\mathbb{P}(\xi\leq\eta)\geq\int_{-\infty}^{\bar{\eta}}f_{\xi}(x)\,\mathrm{d}x=\mathbb{P}(\xi\leq\bar{\eta}),

which achieves the desired result. In order words, if the relaxed chance constraint $\mathbb{P}(\xi\leq\bar{\eta})\geq 1-\delta$ is satisfied, then the original chance constraint $\mathbb{P}(\xi\leq\eta)\geq 1-\delta$ is satisfied as well. ∎

B. Reverse Union Bound

Lemma 4.

Let the events $A_{1},\ldots,A_{n}$ , such that $\mathbb{P}(A_{i})\geq\delta_{i}$ , for some $\delta_{i}\geq 0$ , for all $i=1,\ldots,n$ . Then,

\mathbb{P}\left(\ \bigcap_{i=1}^{n}A_{i}\right)\geq\sum_{i}\delta_{i}-(n-1).

(B.1)

Proof.

From the law of total probability, we have that

\mathbb{P}\left(\ \bigcap_{i=1}^{n}A_{i}\right)=1-\mathbb{P}\left(\bigcup_{i=1}^{n}{A}^{c}_{i}\right),

(B.2)

where ${A}^{c}_{i}$ denotes the complement of the event $A_{i}$ . Note that $\mathbb{P}(A^{c}_{i})\leq 1-\delta_{i}$ for all $i=1,\ldots,n$ . From Boole’s inequality, it follows that

\mathbb{P}\left(\bigcup_{i=1}^{n}{A}^{c}_{i}\right)\leq\sum_{i=1}^{n}\mathbb{P}({A}^{c}_{i}).

(B.3)

Combining (B.2) - (B.3) yields

	$\displaystyle\mathbb{P}\left({\bigcap_{i=1}^{n}A_{i}}\right)$	$\displaystyle\geq 1-\sum_{i=1}^{n}\mathbb{P}({A}^{c}_{i})$
		$\displaystyle\geq 1-\sum_{i=1}^{n}(1-\delta_{i})$
		$\displaystyle=\sum_{i=1}^{n}\delta_{i}-(n-1),$

which achieves the desired result. ∎

C. Input Hard Constrained Covariance Controller

We assume that the hard input constraints on the control are affine, i.e., they are of the form

\alpha_{u,s}^{\intercal}F_{k}U\leq\beta_{u,s},\quad s=1,\ldots,N_{c}.

(C.1)

Theorem 3 ([15]).

The control law

u_{k}=v_{k}+K_{k}z_{k},

(C.2)

where $z_{k}$ is governed by the dynamics

	$\displaystyle z_{k+1}$	$\displaystyle=Az_{k}+\phi(w_{k}),$		(C.3)
	$\displaystyle z_{0}$	$\displaystyle=\phi(y_{0}),\quad y_{0}=x_{0}-\mu_{0},$		(C.4)

where $\phi:\mathbb{R}^{n}\rightarrow\mathbb{R}^{n}$ is an element-wise symmetric saturation function with pre-specified saturation value of the $i$ th entry of the input $y_{i}^{\mathrm{max}}>0$ as

\phi_{i}(y)=\mathrm{max}(-y_{i}^{\mathrm{max}},\mathrm{min}(y_{i},y_{i}^{\mathrm{max}})),

(C.5)

converts Problem 1 to the following convex programming problem that constrains the control to a maximum saturation value

	$\displaystyle\min_{V,K,\Omega}J(V,K,\Omega)=\textrm{tr}\bigg{(}\bar{Q}\begin{bmatrix}I&\mathcal{B}K\end{bmatrix}\Sigma_{XX}\begin{bmatrix}I\\ K^{\intercal}\mathcal{B}^{\intercal}\end{bmatrix}\bigg{)}$
	$\displaystyle+\mathrm{tr}(\bar{R}K\Sigma_{UU}K^{\intercal})+(\mathcal{A}\mu_{0}+\mathcal{B}V)^{\intercal}\bar{Q}(\mathcal{A}\mu_{0}+\mathcal{B}V)+V^{\intercal}\bar{R}V$
	subject to
	$\displaystyle\mathbb{P}(E_{k}X\notin\mathcal{X})\leq\delta_{k},~{}~{}k=1,\ldots,N$		(C.6)
	$\displaystyle\sum_{k=1}^{N}\delta_{k}\leq\Delta,$		(C.7)
	$\displaystyle HF_{k}V+\Omega^{\intercal}\sigma\leq h,$		(C.8)
	$\displaystyle HF_{k}K\begin{bmatrix}\mathcal{A}&\mathcal{D}\end{bmatrix}=\Omega^{\intercal}S,$		(C.9)
	$\displaystyle\Omega\geq 0,$		(C.10)
	$\displaystyle\mu_{f}=E_{N}(\mathcal{A}\mu_{0}+\mathcal{B}V),$		(C.11)
	$\displaystyle\Sigma_{f}\geq E_{N}\begin{bmatrix}I&\mathcal{B}K\end{bmatrix}\Sigma_{XX}\begin{bmatrix}I\\ K^{\intercal}\mathcal{B}^{\intercal}\end{bmatrix}E_{N}^{\intercal},$		(C.12)

where $\Omega\in\mathbb{R}^{2(N+1)n\times N_{c}}$ is a decision (slack) variable,

	$\displaystyle\Sigma_{XX}$	$\displaystyle=\begin{bmatrix}\mathcal{A}&\\ &\mathcal{A}\end{bmatrix}\begin{bmatrix}\Sigma_{0}&\mathbb{E}[y_{0}\phi(y_{0})^{\intercal}]\\ \mathbb{E}[\phi(y_{0})y_{0}^{\intercal}]&\mathbb{E}[\phi(y_{0})\phi(y_{0})^{\intercal}]\end{bmatrix}\begin{bmatrix}\mathcal{A}^{\intercal}&\\ &\mathcal{A}^{\intercal}\end{bmatrix}$
		$\displaystyle+\begin{bmatrix}\mathcal{D}&\\ &\mathcal{D}\end{bmatrix}\begin{bmatrix}I&\mathbb{E}[W\phi(W)^{\intercal}]\\ \mathbb{E}[\phi(W)W^{\intercal}]&\mathbb{E}[\phi(W)\phi(W)^{\intercal}]\end{bmatrix}\begin{bmatrix}\mathcal{D}^{\intercal}&\\ &\mathcal{D}^{\intercal}\end{bmatrix},$		(C.13)

\Sigma_{UU}=\mathcal{A}\mathbb{E}[\phi(y_{0})\phi(y_{0})^{\intercal}]\mathcal{A}^{\intercal}+\mathcal{D}\mathbb{E}[\phi(W)\phi(W)^{\intercal}]\mathcal{D}^{\intercal}.

(C.14)

Further,

	$\displaystyle H$	$\displaystyle=[\alpha_{u,1},\ldots,\alpha_{u,N_{c}}]^{\intercal}\in\mathbb{R}^{N_{c}\times m},$		(C.15)
	$\displaystyle h$	$\displaystyle=[\beta_{u,1},\ldots,\beta_{u,N_{c}}]^{\intercal}\in\mathbb{R}^{N_{c}}.$		(C.16)

In addition, $S\in\mathbb{R}^{2(N+1)n\times(N+1)n}$ and $\sigma\in\mathbb{R}^{2(N+1)n}$ are constant, given by

	$\displaystyle S_{2i-1}$	$\displaystyle=e_{2i-1}^{\intercal},\ S_{2i}=-e_{2i}^{\intercal},$		(C.17)
	$\displaystyle\sigma_{2i-1}$	$\displaystyle=y_{i}^{\mathrm{max}},\ \sigma_{2i}=y_{i}^{\mathrm{max}},$		(C.18)

where $S_{i}$ denotes the $i$ th row of $S$ , and $e_{i}\in\mathbb{R}^{2(N+1)n}$ is a unit vector with $i$ th element 1.

Note that the saturation of the input will result, in general, in a non-Gaussian distribution of the state. As a result, the chance constraint inequalities (III-B) must be replaced by another set of inequalities, for example, of the Chebyshev-Cantelli type [43]. More details can be found in [15]. For the spacecraft rendezvous problem in Section V, it turned out, however, that the original chance constraint formulation worked well, which means that the Chebyshev-Cantelli inequality formulation of the chance constraints may be overly conservative for this problem.


$\displaystyle\\|\bar{\xi}\\|_{2}+$	$\displaystyle\sigma_{\xi}\sqrt{2\log\frac{1}{\delta}}\leq r\Rightarrow\sigma_{\xi}\sqrt{2\log\frac{1}{\delta}}\leq r-\\|\bar{\xi}\\|$	(68a)
	$\displaystyle\Rightarrow 2\sigma_{\xi}^{2}\log\frac{1}{\delta}\leq(r-\\|\bar{\xi}\\|_{2})^{2}$	(68b)
	$\displaystyle\Rightarrow 2\sigma_{\xi}^{2}\log\delta\geq-(r-\\|\bar{\xi}\\|_{2})^{2}$	(68c)
	$\displaystyle\Rightarrow\log\delta\geq-\frac{(r-\\|\bar{\xi}\\|_{2})^{2}}{2\sigma_{\xi}^{2}}$	(68d)
	$\displaystyle\Rightarrow\delta\geq\exp\bigg{(}-\frac{(r-\\|\bar{\xi}\\|_{2})^{2}}{2\sigma_{\xi}^{2}}\bigg{)}$	(68e)
	$\displaystyle\Rightarrow 1-\delta\leq 1-\exp\bigg{(}-\frac{(r-\\|\bar{\xi}\\|_{2})^{2}}{2\sigma_{\xi}^{2}}\bigg{)}.$	(68f)

Covariance Steering with Optimal Risk Allocation

Abstract

I Introduction

II Problem Statement

III Chance Constrained Covariance Steering with Risk Allocation

III-A Lower-Stage Covariance Steering

III-B Polyhedral Chance Constraints

III-C Risk Allocation Optimization

III-D Iterative Risk Allocation Motivation

Theorem 1.

Proof.

III-E Iterative Risk Allocation Algorithm

IV Cone Chance Constraints

IV-A Two-Sided Approximation of Cone Constraints

Proposition 1.

Proof.

IV-A1 Three-cut Outer Approximation

Lemma 1.

IV-A2 Reverse Union Bound Approximation

Proposition 2.

Proof.

IV-B Geometric Approximation

Proposition 3.

Proof.

Lemma 2.

Proof.

Theorem 2.

Proof.

V Spacecraft Rendezvous Example

V-A IRA-CS with Polytopic Chance Constraints

V-B IRA-CS with Cone Chance Constraints

V-C Comparison with MPC-based Controllers

VI Conclusion

VII Acknowledgment

References

A. Cone Chance Constraint Relaxation

Lemma 3.

Proof.

B. Reverse Union Bound

Lemma 4.

Proof.

C. Input Hard Constrained Covariance Controller

Theorem 3 (​​[15]).

Theorem 3 ([15]).