Wasserstein Distributionally Robust Chance Constrained Trajectory Optimization for Mobile Robots within Uncertain Safe Corridor

Safe Corridor based TO has been well studied for collision-free navigation of mobile robots. [2] proposes a semidefinite program-based approach for collision-free UAV navigation, which leverages a greedy finding algorithm for large convex regions of the collision-free space [8]. The efficient operations of the octree-map representation are proposed in [3] to obtain the safe corridor, and the safe corridor constraints are incorporated into a minimum snap TO framework [9]. Similarly, [4] proposes a two-step algorithm to construct the polyhedron-like safe corridor. To guarantee the feasibility of the overall trajectory, [1] further extends [3] by using Bernstein basis polynomial as the trajectory representation, instead of the original discretization points. The time allocation issue of safe corridor-based TO is addressed in [5] through a bilevel optimization approach. In all the above methods, the safe corridor is considered to be deterministic without uncertainty.

In this paper, we extend the safe corridor-based TO from the deterministic formulation to the distributionally robust formulation. In particular, we build on the Bernstein basis method introduced in [1], and provide the distributionally robust chance-constrained counterpart to formally consider the distributional ambiguity of the safe corridor.

Robust TO is proposed to deal with the uncertainty in real-robot applications, such as measurement errors, external disturbances, etc. Set-based approaches provide a safety guarantee for robotic systems with bounded (additional) disturbances. A closely related research topic in the Model Predictive Control (MPC) community is tube-based MPC [10], which tightens the constraints offline, and then online solves an optimization problem as the nominal MPC does. Similar to the idea of tube-based MPC, a robust feedback motion planning method is presented in [11], which computes the outer approximation of the reachable set under uncertainty offline, and then sequentially composes the motion plans online. Chance-constrained optimization [12] provides the basic tools for motion planning of stochastic robot systems. A chance-constrained TO method is proposed in [13] to consider the Gaussian noise in both the robot and obstacle dynamics. A robust cost function in TO is introduced in [14] by computing the ellipsoid bounds around the nominal trajectory. Recently, distributionally robust optimization (DRO) based approaches have been proposed to consider the uncertainty variable without perfectly known probability distributions in robotic motion planning. [15] presents a nonlinear optimal control-based TO method for stochastic dynamics systems with distributionally robust chance constraints, which is computed by sequential quadratic programming. The Wasserstein metric is used in [7] to consider uncertain moving obstacles and a distributionally robust MPC is proposed to achieve a probabilistic guarantee of the out-of-sample risk. However, it is only validated in numerical simulation.

Our method is also a robust TO approach. Different from above-mentioned methods, we leverage DRO techniques to prove our TO problem is a convex QP, making it practically useful for mobile robot applications. In contrast, the above-mentioned methods leverage non-convex optimization problems which are difficult, if not impossible, to obtain the global optima in real-time. As a result, our method can be easily deployed onto real robots, while the above methods are usually limited to simulation. Also, it is worth noting that we mainly focus on the uncertainty of the safe corridor, which is rarely considered in the literature.

The contributions of this paper are summarized as follows:

• •

  We propose a robust trajectory optimization framework considering the distributional ambiguity of the safe corridor for mobile robots.

• •

  Theoretically, we prove that the distributionally robust chance constrained trajectory optimization problem is equivalent to a convex QP.

• •

  By conducting extensive numerical tests, we demonstrate that our methodology has the capacity to significantly decrease the number of infeasible motions in comparison to the nominal approach.

• •

  We evaluate the proposed method on a quadruped robot and a UAV, showing that our method is practically viable for safety-critical robot navigation.

The number of the planning dimension is denoted as $m$. In general, the safe corridor $\mathbb{S}_{c}$ is defined as one of the sub-sets of the passable region $\mathbb{S}_{p}\in\mathbb{R}^{m}$, and is the union set of a sequence of feasible regions $\mathbb{S}_{i}$, i.e.,

$$\mathbb{S}_{c}\triangleq\bigcup\limits_{i=1}^{N}\mathbb{S}_{i}$$

(1)

where the safe corridor consists of $N$ sub-feasible spaces $\mathbb{S}_{i}\in\mathbb{S}_{p}$. In particular, we assume that the sub-feasible spaces are cube-like, and the adjacent spaces have overlapping regions:

$$\mathbb{S}_{i}\cap\mathbb{S}_{i+1}\neq\emptyset,\forall i\in\{1,\cdots,N-1\},$$

$$\mathbb{S}_{i}\triangleq\{\mathbf{p}|\mathbf{p}\in\mathbb{R}^{m},\mathbf{s}^{l}_{i}\leq\mathbf{p}\leq\mathbf{s}^{u}_{i}\},$$

where $\mathbf{p}$ is the position in the motion space, $\mathbf{s}^{l}_{i}\in\mathbb{R}^{m}$ and $\mathbf{s}^{u}_{i}\in\mathbb{R}^{m}$ are the known lower and upper boundaries, respectively. In practice, the sub-feasible spaces can be obtained from an initial path by sampling, graph search or other near-optimal methods [16, 17, 4, 1, 3]. The initial path is piece-wise linear and consists of $N+1$ collision-free path points, denoted as

$$(\mathbf{p}_{0},T_{0}),\cdots,(\mathbf{p}_{i},T_{i}),\cdots,(\mathbf{p}_{N},T_{N})$$

where $\mathbf{p}_{0}$ is the initial position, $T_{0}$ is the start time, $\mathbf{p}_{i}$ is the $i$-th intermediate point and $T_{i}$ is the corresponding arrival time, $\mathbf{p}_{N}$ is the end position, i.e., the goal position. Moreover, the intermediate points are in the corresponding overlapping regions, i.e.,

$$\mathbf{p}_{i}\in\left(\mathbb{S}_{i}\cap\mathbb{S}_{i+1}\right),\ \forall i\in{1,\cdots,N-1}$$

From the initial path, we define the $i$-th piece of trajectory as $B_{i}(t):\mathbb{R}\rightarrow\mathbb{R}^{m}$. Note that $B_{i}(t)$ is related to the $i$-th sub-feasible region $\mathbb{S}_{i}$. Then, if the following constraint holds for $B_{i}(t)$, we can guarantee that $B_{i}(t)$ is collision-free:

$$B_{i}(t)\in\mathbb{S}_{i},\ \forall t\in[T_{i-1},T_{i}],$$

(2)

The trajectory $B_{i}(t)$ has $m$ dimensions, denoted separately by $B_{i,1}(t),\cdots,B_{i,m}(t)$, respectively. Let $B_{i,\mu}(t)$ be an $n$-degree Bernstein basis polynomial, i.e., Bezier curve, for each $\mu\in\{1,\cdots,m\}$. Then $B_{i,\mu}(t)$ could be described by a set of control points:

$$B_{i,\mu}(t)=\sum\limits_{j=0}^{n}c_{i,\mu}^{j}b_{n}^{j}(t),$$

(3)

where $c_{i,\mu}^{j}$ is the $j$-th control point for $B_{i}(t)$ in the $\mu$-th dimension, and $b_{n}^{j}(t)$ is the Bernstein polynomial basis:

$$b_{n}^{j}(t)\triangleq\left(\begin{array}[]{c}n\\ j\end{array}\right)\cdot t^{j}\cdot(1-t)^{(n-j)},$$

where $t\in[0,1]$. The most attractive advantage is the convex hull property of Bezier curves, i.e., a Bezier curve is guaranteed to be within the convex hull of its control points. By leveraging this property, we can prove the following proposition.

Given the initial path, the trajectory optimization problem for mobile robots within deterministic SCCs could be formulated as follows. Please see [1] for a detailed explanation.

	$\displaystyle\displaystyle\min_{\mathbf{c}}$	$\displaystyle\quad\mathbf{c}^{\top}\mathbf{Q}\mathbf{c},$		(5a)
	$\displaystyle{\rm{s.t.}}$	$\displaystyle\quad\mathbf{e}_{\mu}^{\top}{\mathbf{s}}^{l}_{i}\leq c_{i,\mu}^{j}\leq\mathbf{e}_{\mu}^{\top}{\mathbf{s}}^{u}_{i},$		(5b)
		$\displaystyle\quad a_{i,\mu}^{0,j}=c_{i,\mu}^{j},$		(5c)
		$\displaystyle\quad a_{i,\mu}^{l,j}=\frac{n!}{(n-l)!}\cdot(a_{i,\mu}^{l-1,i+1}-a_{i,\mu}^{l-1,i}),$		(5d)
		$\displaystyle\quad a_{1,\mu}^{l,0}\cdot(\tau_{i})^{1-l}=d^{(l)}_{1,\mu},$		(5e)
		$\displaystyle\quad a_{N,\mu}^{l,n}\cdot(\tau_{i})^{1-l}=d^{(l)}_{N,\mu},$		(5f)
		$\displaystyle\quad a_{i,\mu}^{\phi,n}\cdot(\tau_{i})^{1-\phi}=a_{i+1,\mu}^{\phi,0}\cdot(\tau_{i+1})^{1-\phi},$		(5g)
		$\displaystyle\quad a_{\min}^{g}\leq a_{\mu,i-1}^{g,j}(\tau_{i}^{1-\phi})\leq a_{\max}^{g},$		(5h)

where $\mathbf{c}\triangleq[c_{1,x}^{0},\cdots,c_{N,x}^{n},c_{1,y}^{0},\cdots,c_{N,y}^{n},c_{1,z}^{0},\cdots,c_{N,z}^{n}]^{\top}$ is the vector of all control points for all the Bezier pieces, the positive definite matrix $\mathbf{Q}$ is calculated based on [9], $d^{(l)}_{1,\mu}$ and $d^{(l)}_{N,\mu}$ are the given initial and end states where $l$ denotes the $l$-order derivative, $a_{\min}^{g}$ and $a_{\max}^{g}$ are the lower and upper bounds for the $g^{\rm th}$ order derivative of the corresponding piece trajectory, respectively, $j\in\{0,\cdots,n\}$, $i\in\{1,\cdots,N\}$, $\mu\in\{1,\cdots,m\}$, $l\in\{0,\cdots,n\}$, $\phi\in\{0,\cdots,k-1\}$, $g\in\{1,\cdots,k-1\}$.

In Problem (5), the objective function is convex and quadratic, and the constraints are all linear. Therefore, it is indeed a convex quadratic program and can be solved efficiently for robotic applications by many off-the-shelf solvers.

As discussed in Remark 1, the SCCs (4) indeed require the true values of $\mathbf{s}^{l}_{i}$ and $\mathbf{s}^{u}_{i}$. However, it is difficult (if not impossible) to obtain true values due to the errors of robotic sensors or perception algorithms. Therefore, to model the uncertainty of the safe corridor and distinguish from the deterministic case, we take them as random vectors and denote as $\tilde{\mathbf{s}}^{l}_{i}$ and $\tilde{\mathbf{s}}^{u}_{i}$, with their distributions as $\mathbb{P}^{l}_{i}$ and $\mathbb{P}^{u}_{i}$, respectively. However, it is still challenging to measure the exact distributions $\mathbb{P}^{l}_{i}$ and $\mathbb{P}^{u}_{i}$. A popular assumption in probabilistic robotics [18] is to take the distribution of the sensor noise as a Gaussian distribution, but this assumption is too restrictive and possibly inaccurate.

Instead of a specific distribution, in this paper, we assume that $\mathbb{P}^{l}_{i}$ and $\mathbb{P}^{u}_{i}$ are unknown exactly. In particular, we assume that $\mathbb{P}^{l}_{i}$ and $\mathbb{P}^{u}_{i}$ reside in the ambiguity sets $\mathcal{F}(\hat{\mathbb{P}}^{l}_{i},\theta^{l}_{i})$ and $\mathcal{F}(\hat{\mathbb{P}}^{u}_{i},\theta^{u}_{i})$, respectively, both of which are the Wasserstein balls based on the elliptical reference distributions.

We emphasize that Assumption 1 is mild because the distributions $\mathbb{P}^{l}_{i}$ and $\mathbb{P}^{u}_{i}$ under Assumption 1 are distributionally ambiguous, which are less conservative than assuming a known distribution. Note that in particular, knowing the true distributions $\mathbb{P}^{l}_{i}$ and $\mathbb{P}^{u}_{i}$ is a special case of Assumption 1 (by considering the Wasserstein ball as a singleton that only contains the true distribution, i.e., the reference elliptical distribution). Although the elliptical reference distribution $\hat{\mathbb{P}}^{h}_{i}$ is still required to be specified as a prior, the member distributions in the Wasserstein ball can be any type of distribution [19].

Now we are ready to incorporate the distributional ambiguity of the safe corridor into the TO framework. We leverage the idea of distributionally robust chance constraint, that is, even in the worst-case distribution, the $i$-th piece of the trajectory $B_{i}(t)$, which is a Bezier curve, is still in the safe corridor with high confidence $1-\epsilon$. Mathematically, we define the Distributioanllay Robust Safe Corridor Constraints (DRSCCs) for $B_{i}(t)$ as follows:

Note that our distributionally robust chance constraints do not assume the type of the distributions are specified. Moreover, we leverage the distributionally robust optimality to define DRSCC, that is, the chance constraints hold for all the distributions in the ambiguity sets. It is obvious that, if the ambiguity set comprises a single distribution, the DRSCC is simplified to a chance constraint. Therefore, chance-constrained methods can be seen as a special case of our method. By replacing the original SCC constraints (5b) in (5) with DRSCC (7), we obtain the optimization problem for DRSCC-TO:

	$\displaystyle\displaystyle\min_{\mathbf{c}}$	$\displaystyle\quad\mathbf{c}^{\top}\mathbf{Q}\mathbf{c},$		(8a)
	$\displaystyle{\rm{s.t.}}$	$\displaystyle\quad\eqref{drscc_original},$		(8b)
		$\displaystyle\quad\eqref{new_a_1}-\eqref{limit_constraints}.$		(8c)

Clearly, Problem (8) cannot be solved directly because it involves infinitely many constraints in (7). Interestingly, we can prove that it admits a tractable reformulation. Before that, we first define a lower risk threshold for DRSCC:

$$\underline{\epsilon}^{h}_{i}=1-\Phi^{h}_{i}(\eta^{*})$$

(9)

where $h\in\{l,u\}$, $\Phi^{h}_{i}$ is the cumulative distribution function for the reference elliptical distribution $\hat{\mathbb{P}}^{h}_{i}$, and

$$\begin{array}[]{rl}\eta^{*}\triangleq\min&\eta,\\ {\rm{s.t.}}&\eta\geq(\Phi^{h}_{i})^{-1}(1-\epsilon^{h}_{i}),\\ &\eta(\Phi^{h}_{i}(\eta)-(1-\epsilon^{h}_{i}))-\kappa_{i}^{h}\geq\theta^{h}_{i},\\ &\kappa_{i}^{h}=\int_{\frac{1}{2}((\Phi^{h}_{i})^{-1}(1-\epsilon^{h}_{i}))^{2}}^{\eta^{2}/2}k^{h}_{i}g^{h}_{i}(z){\rm{d}}z.\end{array}$$

(10)

where $k^{h}_{i}$ and $g^{h}_{i}(\cdot)$ are the normalizing constant and the generation function for the distribution $\hat{\mathbb{P}}^{h}_{i}$, respectively. Then we have the main theoretical result of this paper:

Note that the problem (11) is still a convex QP. Therefore, similar to the nominal TO (5), we could also solve our DRSCC-TO (11) in real-time for robotic applications.

In numerical simulation, we compare our DRSCC-TO method with the nominal TO method in terms of optimal costs and violation count. We focus on 2D minimum snap trajectory optimization [9] and proceed as follows.

First, we establish three nominal safe corridors, which are known for both nominal TO and DRSCC-TO. For DRSCC-TO, we employ different reference distributions: normal distribution, t-distribution, and logistic distribution. For these ellipsoidal distributions, the mean vectors align with corner positions of nominal safe corridors, and the covariance matrices are $\sigma I$, with $\sigma$ values being $2$, $1$, and $1$ for each distribution. Then, we formulate the problems (5) and (11) for nominal TO and DRSCC-TO. Solving them produces optimized trajectories. DRSCC-TO employs consistent Wasserstein distance and confidence level for each sub-region and dimension. We compare six parameter sets for DRSCC-TO ($d\in\{0.05,0.1\}$, $\epsilon\in\{0.1,0.15,0.25\}$). Finally, nominal safe corridors are perturbed to assess method robustness. We sample $\psi_{1}$ from the prescribed reference distribution and $\psi_{2}$ from a uniform distribution. The instance $\psi=(1-\alpha)\psi_{1}+\alpha\psi_{2}$ is obtained using weight $\alpha\in[0,1]$, with five $\alpha$ values and 2000 instances per value, resulting in 10000 perturbed safe corridors in each test.

We calculate optimal costs for both methods across different demonstrations and the results are summarized in Table I. DRSCC-TO exhibits significantly fewer violations than nominal TO. Especially in Case III, DRSCC-TO with logistic distribution ($\theta=0.5$, $\epsilon=0.1$) rarely shows infeasible motions compared to nominal TO. Also, optimal cost decreases with smaller $d$ or larger $\epsilon$, but violations increase. In conclusion, our method improves safety by reducing violations when safe corridors are perturbed.

The trade-off is that our method’s optimal cost exceeds nominal TO’s. This implies our method’s control inputs are more energetic. However, safety generally takes precedence over efficiency for autonomous robots, as infeasible motions risk hardware damage (e.g., micro drones colliding with obstacles).

In this section, we further validate our method on a micro UAV and a quadruped robot to show the practical viability of our method.

Notice that (7a) is equivalent to

$$\sup_{\mathbb{P}_{i}^{l}\in\mathcal{F}(\hat{\mathbb{P}}^{l}_{i},\theta^{l}_{i})}\mathbb{P}_{i}^{l}(\mathbf{e}_{\mu}^{\top}\tilde{\mathbf{s}}^{l}_{i}>c^{j}_{i,\mu})\leq\epsilon,$$

where the strict inequality can be replaced by a weak one and we have

$$\sup_{\mathbb{P}_{i}^{l}\in\mathcal{F}(\hat{\mathbb{P}}^{l}_{i},\theta^{l}_{i})}\mathbb{P}_{i}^{l}(\mathbf{e}_{\mu}^{\top}\tilde{\mathbf{s}}^{l}_{i}\geq c^{j}_{i,\mu})\leq\epsilon,$$

(12)

see, e.g., proposition 3 in [23]. To proceed, we kindly remind the definition of the value-at-risk:

$$\mathbb{P}\text{-VaR}_{1-\epsilon}(\tilde{s})=\inf_{x\in\mathbb{R}}\{x\;|\;\mathbb{P}(\tilde{s}>x)\leq\epsilon\}$$

with risk threshold $\epsilon\in(0,0.5)$. Then, we can rewrite (12) as:

$$\sup_{\mathbb{P}_{i}^{l}\in\mathcal{F}(\hat{\mathbb{P}}^{l}_{i},\theta^{l}_{i})}\mathbb{P}_{i}^{l}(\mathbf{e}_{\mu}^{\top}\tilde{\mathbf{s}}^{l}_{i}-c^{j}_{i,\mu}\geq 0)\leq\epsilon,$$

(13)

which is equivalent to

$$\sup_{\mathbb{P}_{i}^{l}\in\mathcal{F}(\hat{\mathbb{P}}^{l}_{i},\theta^{l}_{i})}\mathbb{P}^{l}_{i}\text{-VaR}_{1-\epsilon}(\mathbf{e}_{\mu}^{\top}\tilde{\mathbf{s}}^{l}_{i}-c^{j}_{i,\mu})\leq 0.$$

(14)

Note that the equivalence follows immediately from the definition of the value-at-risk. Due to the choice of the Mahalanobis norm, we can establish the equivalence between (14) and

$$\mathbb{P}_{\boldsymbol{\mu}^{l}_{i},\boldsymbol{\Sigma}^{l}_{i},g^{l}_{i}}(\mathbf{e}_{\mu}^{\top}\tilde{\mathbf{s}}^{l}_{i}\leq c^{j}_{i,\mu})\geq 1-\underline{\epsilon}^{l}_{i}.$$

(15)

Please see corollary 4.9 in [24] and also [25] for the details of the proof of the equivalence. Remember in Assumption 1 that the reference distribution is assumed to be an elliptical distribution $\hat{\mathbb{P}}^{l}_{i}=\mathbb{P}_{\boldsymbol{\mu}^{l}_{i},\boldsymbol{\Sigma}^{l}_{i},g^{l}_{i}}$. Also, from (9) and (10), we have:

$$\begin{array}[]{rl}\underline{\epsilon}^{l}_{i}&=1-\Phi^{h}_{i}(\eta^{*})\\ &\leq 1-\Phi^{h}_{i}((\Phi^{h}_{i})^{-1}(1-\epsilon^{l}_{i}))\\ &=\epsilon^{l}_{i}<0.5.\end{array}$$

(16)

Therefore, we have the equivalences as follows:

$$\begin{array}[]{rl}&\displaystyle\hat{\mathbb{P}}^{l}_{i}(\mathbf{e}_{\mu}^{\top}\tilde{\mathbf{s}}^{l}_{i}\leq c^{j}_{i,\mu})\geq 1-\underline{\epsilon}^{l}_{i}\vspace{1mm}\\ \Longleftrightarrow&\mathbb{P}_{\boldsymbol{\mu}^{l}_{i},\boldsymbol{\Sigma}^{l}_{i},g^{l}_{i}}(\mathbf{e}_{\mu}^{\top}\tilde{\mathbf{s}}^{l}_{i}\leq c^{j}_{i,\mu})\geq 1-\underline{\epsilon}^{l}_{i}\vspace{1mm}\\ \Longleftrightarrow&\Phi\left(\frac{c^{j}_{i,\mu}-\mathbf{e}_{\mu}^{\top}\bm{\mu}^{l}_{i}}{\sqrt{\mathbf{e}_{\mu}^{\top}\mathbf{\Sigma}^{l}_{i}\mathbf{e}_{\mu}}}\right)\geq 1-\underline{\epsilon}^{l}_{i}\vspace{1mm}\\ \Longleftrightarrow&c^{j}_{i,\mu}\geq\mathbf{e}_{\mu}^{\top}\boldsymbol{\mu}^{l}_{i}+\sqrt{\mathbf{e}^{\top}_{\mu}\mathbf{\Sigma}_{i}^{l}\mathbf{e}_{\mu}}(\Phi^{l}_{i})^{-1}(1-\underline{\epsilon}^{l}_{i}),\end{array}$$

where the second equivalence holds for the linearity of elliptical distributions, and the third one is due to (16). Similarly, we can prove that (7b) is equivalent to:

$$c^{j}_{i,\mu}\leq\mathbf{e}_{\mu}^{\top}\boldsymbol{{\mu}}^{u}_{i}-\sqrt{\mathbf{e}^{\top}_{\mu}\mathbf{\Sigma}_{i}^{u}\mathbf{e}_{\mu}}(\Phi^{u}_{i})^{-1}(1-\underline{\epsilon}^{u}_{i}),$$

Therefore, we conclude that the problem (8) is equivalent to problem (11).