suffer from the lack of convergence guarantees [1]. Successive convexification-based methods provide convergence guarantees, but they may still suffer from scalability issues, if the underlying system dynamics are stochastic [2], [3], [4].

Dynamic programming-based algorithms have been proposed for unconstrained stochastic trajectory optimization problems to alleviate the scalability issue in [5], [6] and for constrained problems in [7], [8]. These methods, however, cannot guarantee safety and their applicability is limited to smooth objective functions. On the other hand, samplingbased stochastic optimization algorithms deal with nonsmooth objective functions but they often cannot handle unmodeled disturbances and model mismatch [9], [10].

The two papers that are most closely related to our approach are [11], [12]. In [11], the authors use Covariance Steering for path planning for linear systems with chance constrained obstacle avoidance. However, the approach in [11] does not scale well due to the fact that the number of decision variables increases quadratically with the problem horizon because of the feedback terms. Furthermore, integer variables that are used to encode obstacle avoidance constraints add to computational complexity. In addition, the quality of the solution depends on the decomposition of the safe region into convex polytopes, which is in general a complex problem. In [12], the authors use unconstrained covariance steering to take sample trajectories from the low cost regions of the state space to enhance the performance and to avoid local optima. However, this method requires the terminal mean and covariance as design parameters, which can be hard to tune, and the safety constraints are not explicitly satisfied. Main Contributions: This paper presents a novel trajectory optimization algorithm (CCSMPPI) for stochastic linear systems with a non-convex safe state space. CCSMPPI solves the stochastic trajectory optimization problem with guaranteed satisfaction of the safety constraints, which cannot be achieved by standard MPPI. It does so by combining the standard MPPI with the CCS. In particular, MPPI is used to generate a reference trajectory, which is then used to generate a convex safe region, by solving an unconstrained stochastic trajectory optimization problem whereas CCS generates a control policy which minimizes the divergence from the reference trajectory while satisfying the safety constraints. In this way, CCSMPPI endows the MPPI algorithm with robustness to stochastic disturbances by leveraging the framework of CCS. These disturbances may cause the system to diverge from the computed trajectory and may cause violation of safety constraints. This problem is more frequently observed when the cost function is non-smooth such as the sum of indicator functions [10]. CCSMPPI satisfies the probabilistic safety constraints by taking the distribution of disturbances into account.

The final improvement of the CCSMPPI over the standard MPPI is the robustness and the safety guarantees against poorly designed cost functions and incorrect tuning of algorithm parameters which are common issues in MPPI. The parameter values and the cost functions that are tuned for one scenario by the designer may not work well in other scenarios and MPPI may return unsafe control inputs. The CCS procedure of the algorithm filters the unsafe inputs that are computed by MPPI and corrects them by means of a feedback control law. This technique makes the cost function design and parameter tuning tasks less time-consuming and allows designers to experiment with cost functions and parameters more freely without compromising safety.

Finally, the practicality of CCSMPPI is demonstrated in 2 different trajectory optimization problems in which we compare our results with those obtained by using the standard MPPI [9] and tube-MPPI [10]. It is shown in numerical experiments that our approach is superior to standard MPPI and tube-MPPI in terms of providing safety against both stochastic disturbances and poorly designed cost functions.

We denote by R n the set of n-dimensional real vectors, by R + the set of non-negative real numbers and by N + the set of non-negative integers. We write E [•] to denote the expectation functional. We denote the probability of the random event E as P(E). Given a vector x, its 2-norm is denoted by x 2 and given a matrix A, we denote its Frobenius norm by A F if A is a square matrix, we denote its trace by tr(A). We use 0 and I n to denote the zero matrix with suitable dimensions and the n × n identity matrix, respectively. We will denote the convex cone of n × n symmetric positive semi-definite (symmetric positive definite) matrices by S + n (S ++ n ). We write bdiag(A 1 , . . . , A ) to denote the block diagonal matrix formed by the matrices A i , i ∈ {1, . . . , }. We use ∪ N i=1 O i to denote the union of sets O i indexed by i ∈ {1, . . . , N }. We denote by µ z and var z the mean and the variance of a random vector z, respectively, that is,

We use N (µ, Σ) to denote normally distributed random variable with mean µ covariance Σ.

We consider a discrete-time stochastic linear system with dynamics:

where

is the control input and w k ∈ R 4 is the disturbance. We assume that w k ∼ N (0, W k ) and E[w k w T l ] = 0 for all k = l. In our problem formulation, we consider that the obstacles lie in the position space R 2 .

The choice of the objective function will be determined by the particular application. However, in this paper, we use the objective function utilized in the formulation of the information-theoretic MPPI [9], which is defined as follows:

where X N = {x 0 , x 1 , . . . , x N } is the state sequence, U N -1 = {u 0 , u 1 , . . . , u N -1 } is the control input sequence, q : R n → R + is the state-dependent term of the running cost function, Φ : R 2n → R + is the terminal cost function, R k ∈ S + n and λ ∈ R + . While the term λu T k R k u k penalizes the control input, q(x k ) and Φ(x N ) are task dependent and each of them can either be a smooth function as in [9] or a sum of indicator functions for obstacle avoidance as in [10].

We assume that the position space is populated by N obs obstacles. The i-th obstacle is parametrized by its position, s i ∈ R n , and its radius, r i ∈ R + , and the region it occupies is denoted as O i , where

The position space is defined as X ⊆ R n and the safe region (or obstacle-free region) of the position space is defined as X safe := X \O where O = ∪ N obs j=1 O j . Then, the safe trajectory optimization problem can be formally stated as follows: minimize

where I N = {0, 1, . . . , N }, x0 ∈ R 2n and P fail ∈ (0, 0.5] is the acceptable level of the probability of the failure.

It should be highlighted here that since the dynamics of the system include the random noise term w k , the state sequence X k will be a random process, and the problem that we will be trying to solve will correspond to an instance of a stochastic trajectory optimization problem.

Control (MPC) algorithm [9]. It works by taking K samples of control sequences from a Gaussian distribution, and finding the corresponding state trajectories and costs. Each sequence is then weighted by an exponential transform of its cost and the optimal control sequence is found as the weighted sum, written as:

where

k ∼ N (0, νI) and C i is defined as follows:

C i is the total path cost induced by ith sample trajectory, v = [v 0 , . . . , v TMPPI-1 ] T is the optimal control sequence obtained by the previous iteration of the MPPI, (i) is the control sampling noise, u (i) is the ith control sequence sample and ν ∈ R + is the control sampling covariance parameter. At the next iteration, the previous optimal control sequence is shifted in time and used as the mean of the Gaussian distribution to sample controls from. MPPI may not perform well when the system has a disturbance that causes control sampling distribution to only sample high cost trajectories. Tube-MPPI [10] addresses this by running MPPI from two starting states, a nominal and a real state. The real state is taken from the state of the real system as before whereas the nominal state is found by propagating the previous nominal state and control through the noise-free dynamics model without the disturbance term, w k . Once the MPPI optimization step is done, the optimal trajectory of the real system is pushed to follow the optimal trajectory of the nominal system through the use of a feedback controller. The nominal state and control sequence can then be reset to the real system's when the difference in the free energy of the real and nominal systems is below a user-defined threshold. This means that Tube-MPPI should perform similarly to MPPI in most cases as the nominal and real states will be nearly equivalent but in cases where a large disturbance affects the real system, the nominal system should provide a control sequence unaffected by the disturbance that the real system can then track to get out of a high-cost region.

The main objective of the constrained covariance steering (CCS) problem is to steer the mean and the covariance of a stochastic linear system to desired values while minimizing the expected value of an objective function subject to state and/or input constraints [13], [14]. The general form of the discrete-time CCS problem can be formally stated as follows:

where Π denotes the set of causal policies, X ⊆ R nx and U ⊆ R nu are arbitrary sets corresponding to state and input constraints. The constraints defined in (9d) represents the initial state distribution and the desired terminal state distribution. We should point out that the terminal covariance constraint in (9d) is dropped in our constrained covariance steering formulation since it is not useful to specify a desired covariance for safety as long as constraints in (9c) are satisfied. Discrete-time formulations of the CCS problems can be cast as finite-dimensional deterministic optimization problems by restricting the class of admissible policies to those which admit the affine state history feedback parametrization [15] or the disturbance feedback parametrization [16] (both tailored to the covariance steering problem). In this work, we will be utilizing the latter parametrization according to which the control input at each discrete stage can be expressed as follows:

where H k , K k ∈ R n×n and vk ∈ R n for all k ∈ {0, 1, . . . , N -1}. This parametrization allows us to cast the covariance steering problem as a finite-dimensional (deterministic) optimization problem in terms of the following decision variables:

In order to do that, the decision variables are represented in a more compact form as follows:

T . Then, it follows from (1) that

where x0 := x 0 -µ 0 . Equations (11a)-(11b) are derived from ( 1) and ( 10) respectively. The reader can refer to [15], [16] for the details of the previous derivations. We can compute the mean of the vectors x and u by taking the expectation of both sides of (11a) and (11b). Then, we compute the deviation of concatenated vectors x = xµ x and ṽ := vµ v . Finally, we compute the variances var x := E[xx T ] and var v := E[ṽṽ T ] as follows:

where

xT 0 ] and W = bdiag(W 0 , . . . , N -1). Furthermore, the state and the control at the discrete stage k can be recovered from the concatenated state and input vector as where F x k and F v k denote the block matrices whose kth block is equal to the identity matrix and the other blocks are equal to zero. Thus, the mean and the covariance of x k and v k are given by:

So, we can express the mean of x k and v k as affine functions of the decision variable v whereas the covariance matrices of

x k and v k can be expressed as convex quadratic functions of decision variables H and K. This allows us to cast various forms of the (constrained) covariance steering problem as convex optimization problems which can be solved with highly efficient solvers. The reader can refer to [15], [16], [17], [18] for more details.

V. MAIN ALGORITHM The main components of the algorithm are the MPPI controller, the half-space generator, and the Constrained Covariance Steering module. The MPPI controller solves the unconstrained stochastic trajectory optimization problem and returns a state and an input sequence of length T MPPI . The state sequence generated by the MPPI module is used to generate the half-space constraints. The state and the input sequences and half-space constraints are used in the CCS module to solve for a policy that is guaranteed to be safe with high probability.

A. MPPI In this paper, we follow the procedures described in [9] to use MPPI algorithm. The MPPI module requires system dynamics and initial state x 0 to sample trajectories. The MPPI horizon T MPPI , the input sampling covariance parameter ν, the number of trajectory samples K are required as algorithm parameters. The input cost matrix R k 0, the state-dependent term of the running cost function q(x k ), and the terminal cost function Φ(x) are taken as problem data, and they are chosen according to the task at hand.

Safe half-spaces are generated by the "HSGen" procedure which takes the obstacle information tuple M obs , the reference state sequence X TMPPI , and the T CS as inputs. It takes the first T CS states of the sequence X TMPPI and projects the position vectors p onto each obstacle O j . Then, it computes the supporting hyperplane:

at the point of projection such that

The procedure of half-space generation is illustrated in Figure 2. The projection of position p at time onto the obstacle O j is denoted as z ,j and is defined as follows:

where h ,j := (ps j )/ ps j 2 is the unit normal vector to obstacle O j at the point z ,j pointing towards p . We set a ,j = h ,j and, b ,j should satisfy a T ,j z ,j -b ,j = 0. So, we can express a ,j and b ,j in terms of p l , s j and r j as follows:

The halfspace generation process is repeated for each obstacle O j where j ∈ {1, . . . , N obs } and every time step ∈ {0, 1, . . . , T CS }. The halfspace parameters are gathered in the set of tuples S obs k = {(a ,j , b ,j )} =0,...,T M C ;j=1,...,N obs to be used in Constrained Covariance Steering.

We should also point out that p / ∈ O j in Figure 2 but this condition is not necessary for the half-space generation procedure. Even if p ∈ O j , the procedure described by equations in (17) generates a half-space H ,j such that a T ,j pb ,j ≤ 0 holds for all p ∈ O j .

The goal of the Constrained Covariance Steering Module is to minimize the deviation of the actual state and control sequence from the reference state and control sequence which is computed by the MPPI algorithm while satisfying the safety constraints. This problem can be formally stated as the following stochastic optimal control problem:

where Π denotes the set of all admissible control policies, δx = xx MPPI , δu = uu MPPI , I t := {0, . . . , T CS }, I o := {1, . . . , N obs }, I = I t × I o . The stochastic optimal control problem defined in (18a), (18b) and (18d) can be cast as a deterministic optimization problem by fixing the policy as in (10) and concatenating the states {x k } N k=0 , the inputs {u k } N -1 k=0 and the random noise vectors {w k } N -1 k=0 as explained in Section IV. The resulting finite dimensional deterministic optimization problem is given by: minimize

where RR T = bdiag(Σ 0 , W), x and u are defined as in Section IV,

). P is defined such that p = P x and α = ϕ -1 (1-P fail ) where ϕ and Σ 0 = 0 where x0 and Σ 0 represent the initial nominal state and initial covariance respectively. Then, using xk as the initial state, MPPI generates a pair of reference state and input sequences (X TMPPI , U TMPPI ). The state sequence is used to generate a safe convex region over which the constraints (18d) are satisfied based on the technique that will be described in Section V-B. Then, we formulate a corresponding CCS problem that seeks for a control policy in the form of (10). This control policy will guarantee collision avoidance while minimizing the deviation from the state and input sequences generated by the MPPI module. If the largest eigenvalue of the computed covariance Σ k+1 exceeds a predetermined threshold σ max , then the nominal state xk is set equal to the real state x k and covariance Σ k is set to 0. Next, the nominal state xk+1 and covariance matrix Σ k+1 will be updated as described in lines 9-10 in Algorithm 1.

It is worth mentioning that the CCS module uses the disturbance noise covariance in its formulation. However, this information is usually unknown in real-world scenarios. But, this can easily be handled by over-approximating the noise covariance, that is, by taking W k W real k , where W real k is the actual noise covariance that is acting on the system, and the previous inequality should be understood in the Loewner partial ordering sense. This allows the CCS module to find a policy that satisfies the safety constraints. Although this approach may generate overly conservative policies, system identification techniques can be used to learn the actual noise covariance [21] and hence reduce conservativeness.

The half-space generation module typically underapproximates the safe region for a time step k. That is, the generated safe set is a subset of the actual safe region. If the real covariance W real k of the disturbance action on the system w k satisfies the matrix inequality W k W real k , then the satisfaction of the constraint (18d) implies that the state will stay in the safe region with probability greater than 1 -P fail .

If the trajectory that is returned by the stochastic optimization module violates safety constraints, the half-space generation module might return half-spaces that are very different in consecutive time steps, which makes the CCS problem defined in (19) infeasible. To avoid this potential problem, we use the constrained covariance steering formulation for only the first few time steps of the trajectory optimization algorithm (T max > T CS ). If xk ∈ X safe , then the safety violations in the first T CS time steps would be small and the half-spaces between consecutive time steps would be close to each other i.e. a ,ja +1,j 2 ≤ ε and b ,jb +1,j 2 ≤ ε for some small ε > 0.

The final component of our algorithm is the use of nominal dynamics, which are the same as real dynamics except it is noise-free. Since the CCS module assures that chance constraints are satisfied with high probability and it uses the nominal state as the initial mean state in its formulation, the nominal state will be safer than the real state. This justifies the use of the nominal state as the initial state in the trajectory optimization module. Also, by computing the covariance in line 10, we compute the high probability region where the real state lies. Then, this covariance value is used as initial covariance in the CCS procedure in the next step to guarantee the satisfaction of the chance constraints with high probability. This procedure also allows us to use the feedback term computed in the CCS because if µ 0 and Σ 0 are set to x k and 0 respectively, then feedback term H would be equal to 0 and consequently the covariance steering would have little effect.

In our numerical experiments, we consider a double integrator with dynamics described by (1) with:

dt = 0.05, and {w k } is taken to be a white noise process with w k ∼ N (0, W k ) where the noise covariance matrix W k varies depending on different problem instances. We show the efficacy of our approach in two trajectory optimization problems: an obstacle avoidance problem and a path generation problem in a circular track.

Obstacle Avoidance: In the obstacle avoidance case, we compare the performances of CCSMPPI with tube-MPPI [10] under high noise that is acting upon the system to show the robustness of our approach against stochastic disturbances. In the tube-MPPI formulation, an LQG tracking controller is used to track nominal state and input sequences.

In our experiments, the LQG cost function parameters

are chosen to be equal to the cost function parameters used in the CCS formulation given in (18a). Also, the failure parameter P fail that is shown in (18d) is taken to be 0.01.

To show that CCSMPPI guarantee safety against stochastic disturbances, we consider the running cost function q(x k ) to be equal to q hard which is defined as follows:

and Φ hard (x T ) = 0 where I Oj : R 2 → {0, 1} is the indicator function of set O j . The parameters of the MPPI algorithm used in the experiments which produce the results shown in Figure 3 and Figure 4, are T MPPI = 40, K = 100, λ = 0.1, ν = 0.1 and k ∼ N (0, 0.001I). In addition, the problem horizon parameter T max = 200 and the noise covariance matrix W k = bdiag(0., 0., 5.0, 5.0). In these experiments, the statedependent term of the running cost function was taken to be q(x k ) = 10 q hard (x k ) and the desired final position p des = [2.0, 10.0] T . Figure 3 illustrates 10 randomly sampled trajectories induced by the CCSMPPI algorithm. Although the intensity of the noise that is acting upon the system is quite high compared to the sampling distribution parameter ν, the CCSMPPI is successfully avoiding obstacles. Figure 4 shows 10 randomly sampled trajectories of the system running under the tube-MPPI algorithm. It can be seen that the agent reaches the goal position but fails to avoid obstacles even though q hard (x k ) is used as running cost, and the tube-MPPI and standard MPPI statistically by sampling N sim = 15 trajectories for both experiments #1 and #2. The running cost function is taken as 100q t,s (x k ) and 100q t,h (x k ) in experiments #1 and #2, respectively. Also, T max is taken as 200 and 300 in experiments #1 and #2, respectively. In both experiments, W k are chosen to be equal to bdiag(0.005, 0.005, 0.5, 0.5) and v des = 6.0. Pr fail represents the probability of failure and it is computed by dividing the number of trajectories that leave the circular track at least once (N fail ) by total number of trajectories N sim .

It can be seen from the results of experiment #1 in Table I that standard MPPI performs better in terms of minimizing the cost than both tube-MPPI and CCSMPPI and reaches higher speeds. However, this is due to the poor design of the cost function, and the fact that the control inputs that are corrected by CCS module to guarantee safety are not optimal with respect to the used cost function. When q t,h (x k ) is used as the running cost in experiment #2, standard MPPI performs worse than both tube-MPPI and CCSMPPI due to the presence of random noise w k . In these experiments, the safety of the trajectory is the first priority, as encoded in the running cost q t,h (x k ). Although tube-MPPI reaches higher speeds, it fails to reach the safety levels of CCSMPPI. Thus, we can conclude that CCSMPPI is superior to standard MPPI and tube-MPPI in terms of minimizing safety violations.

It should be highlighted that the probability of violating the constraint in (18d) at every time step k is less than P fail = 0.01 but still greater than 0. This means that as T max → ∞, the failure probability of a trajectory approaches 1. This is the reason why Pr fail is non-zero for CCSMPPI in both experiments. Pr fail can be reduced by lowering the safety threshold P fail , however it is not possible to make it 0 since w k is assumed to be normally distributed which is unbounded. In this paper, we presented a novel framework for safe trajectory optimization for stochastic linear systems. Our method mainly consists of three components which are a stochastic optimization algorithm, a convex safe region generator, and a constrained covariance steering algorithm. In particular, we used Model Predictive Path Integral (MPPI) control for stochastic optimization and a projection-based linearization method for the generation of safe convex regions. In addition, we used a Constrained Covariance Steering algorithm based on the affine disturbance feedback parametrization to safeguard against unmodeled noise disturbances that the MPPI algorithm may not always handle satisfactorily. Our numerical simulations have demonstrated that our approach can guarantee safety against unmodeled noise uncertainties as well as unsafe outputs generated by the stochastic optimization algorithm. In our future work, we plan to extend our proposed framework to trajectory generation problems for uncertain nonlinear systems based on model-free trajectory optimization algorithms while guaranteeing safety by utilizing nonlinear covariance steering algorithms.