Chance-constrained Collision Avoidance for MAVs in Dynamics Environments

Posted on 2020-06-30 Edited on 2021-08-16 In Chance Constraints

Chance-constrained Collision Avoidance for MAVs in Dynamics Environments

Chance-constrained nonlinear model predictive control (CCNMPC)

assume uncertainties as Gaussian distribution, transform chance constraints into deterministic constraints

Model:

robot model:
$$
\mathbf{x}{i}^{k+1}=\mathbf{f}{i}\left(\mathbf{x}{i}^{k}, \mathbf{u}{i}^{k}\right)+\omega_{i}^{k}, \quad \mathbf{x}{i}^{0} \sim \mathcal{N}\left(\hat{\mathbf{x}}{i}^{0}, \Gamma_{i}^{0}\right),\quad \omega_i^k\sim\mathcal{N}(0,Q_i^k)
$$
where $\mathbf{x}{i}^{k}=\left[\mathbf{p}{i}^{k}, \mathbf{v}{i}^{k}, \phi{i}^{k}, \theta_{i}^{k}, \psi_{i}^{k}\right]^{T} \in \mathcal{X}{i} \subset \mathbb{R}^{n{x}}$, the mean and covariance of initial state$\mathbf{x}_i^0$ are obtained by state estimator (UKF).
Obstacle model:

$o\in \mathcal{I}_0={1,2,\cdots,n_0} \subset \mathbb{N}$ at position $\mathbf{p}_o \in \mathbb{R}^3$, can be represented by ellipsoid $S_0$ with semi-principal axes $(a_0,b_o,c_o)$ and rotation matrix $R_o$.

moving obstacles has constant velocity with Gaussian noise $\omega_o(t)\sim\mathcal{N}(0,Q_o(t))$ in acceleration, i.e. $\ddot{\mathbf{p}}_o=\omega_o(t)$. estimate and predict the future position and uncertainty by linear KF.
Collision Chance Constraints:
1. Collision condition:
  - collision of robot $i$ with robot $j$
    $$
    C_{i j}^{k} :=\left{\mathbf{x}{i}^{k} |\left|\mathbf{p}{i}^{k}-\mathbf{p}{j}^{k}\right| \leq r{i}+r_{j}\right}
    $$
  - collision of robot $i$ with obstacle $o$:
    
    obstacle: enlarged ellipsoid and check if the robot is inside it.
    $$
    C_{i o}^{k} :=\left{\mathbf{x}{i}^{k} |\left|\mathbf{p}{i}^{k}-\mathbf{p}{o}^{k}\right|{\Omega_{i o}} \leq 1\right},\quad
    \Omega_{i o}=R_{o}^{T} \operatorname{diag}\left(1 /\left(a_{o}+r_{i}\right)^{2}, 1 /\left(b_{o}+r_{i}\right)^{2}, 1 /\left(c_{o}+r_{i}\right)^{2}\right) {R_{o}}
    $$
2. Chance constraints:
  $$
  \begin{array}{ll}{\operatorname{Pr}\left(\mathbf{x}{i}^{k} \notin C{i j}^{k}\right) \geq 1-\delta_{r},} & {\forall j \in \mathcal{I}, j \neq i} \ {\operatorname{Pr}\left(\mathbf{x}{i}^{k} \notin C{i o}^{k}\right) \geq 1-\delta_{o},} & {\forall o \in \mathcal{I}_{o}}\end{array}
  $$
Problem Formulation:

optimization problem with $N$ time steps and planning horizon $\tau=N\Delta t$, $\Delta t$ is time step.
$$
\begin{aligned} \min {\hat{x}{i}^{1}, \cdots, u_{i}^{0}, N-1} \quad & \sum_{k=0}^{N-1} J_{i}^{k}\left(\hat{\mathbf{x}}{i}^{k}, \mathbf{u}{i}^{k}\right)+J_{i}^{N}\left(\hat{\mathbf{x}}{i}^{N}\right) \ \text { s.t. }\quad & \mathbf{x}{i}^{0}=\hat{\mathbf{x}}{i}(0), \quad \hat{\mathbf{x}}{i}^{k}=\mathbf{f}{i}\left(\hat{\mathbf{x}}{i}^{k-1}, \mathbf{u}{i}^{k-1}\right) \ & \operatorname{Pr}\left(\mathbf{x}{i}^{k} \notin C_{i j}^{k}\right) \geq 1-\delta_{r}, \forall j \in \mathcal{I}, j \neq i \ & \operatorname{Pr}\left(\mathbf{x}{i}^{k} \notin C{i o}^{k}\right) \geq 1-\delta_{o}, \forall o \in \mathcal{I}{o} \ & \mathbf{u}{i}^{k-1} \in \mathcal{U}{i}, \quad \hat{\mathbf{x}}{i}^{k} \in \mathcal{X}_{i} \ & \forall k \in{1, \ldots, N} \end{aligned}
$$
Compute uncertainty propagation

Methods:

Y. Z. Luo and Z. Yang, “A review of uncertainty propagation in orbital mechanics,” Progr. Aerosp. Sci., vol. 89, pp. 23–39, 2017.

Unscented transformation

A.Vo ̈lzandK.Graichen,“Stochastic model predictive control of nonlinear continuous-time systems using the unscented transformation,” inProc. Eur. Control Conf., 2015, pp. 3365–3370.
- polynomial chaos expansions
A. Mesbah, S. Streif, R. Findeisen, and R. D. Braatz, “Stochastic nonlinear model predictive control with probabilistic constraints,” in Proc. Amer. Control Conf., 2014, pp. 2413–2419.

These methods are computational intensive and only outperform linearization methods when the propagation time is very long.

In this paper, propagate the uncertainty using EKF-type update.
$$
\Gamma_{i}^{k+1}={F_{i}^{k}} {\Gamma_{i}^{k}} {F_{i}^{k}}^{T}+Q_{i}^{k}
$$
${\Gamma_{i}^{k}}$ is the state uncertainty covariance at time $k$, $F_{i}^{k}=\left.\frac{\partial \mathbf{f}{i}}{\partial \mathbf{x}{i}}\right|{\hat{\mathbf{x}}{i}^{k-1}, \mathbf{u}_{i}^{k}}$ is the state transition matrix of robot.

Chance Constraints:

Idea: linearize the collision conditions to get linear chance constraints. Then reformulate them into deterministic constraints

Linear Constraints:
$$
\operatorname{Pr}(\mathbf{a}^T \mathbf{x}\leq b) \leq \delta
$$
$\mathbf{x}$ Follows Gaussian distribution, the chance constraint can be transformed into deterministic constraint

Lemma 1:

Given a multivariate random variable $\mathbf{x}\sim\mathcal{N}(\hat{\mathbf{x}},\Sigma)$, then
$$
\operatorname{Pr}\left(\mathbf{a}^{T} \mathbf{x} \leq b\right)=\frac{1}{2}+\frac{1}{2} \operatorname{erf}\left(\frac{b-\mathbf{a}^{T} \hat{\mathbf{x}}}{\sqrt{2 \mathbf{a}^{T} \Sigma \mathbf{a}}}\right)
$$
where $\operatorname{erf}(\cdot)$ is standard error function as $\operatorname{erf}(x)=\frac{2}{\sqrt{\pi}}\int_0^x e^{-t^2} dt$.

Lemma 2:

Given a multivariate random variable $\mathbf{x}\sim\mathcal{N}(\hat{\mathbf{x}},\Sigma)$ and a probability threshold $\delta\in(0,0.5)$, then
$$
\operatorname{Pr}\left(\mathbf{a}^{T} \mathbf{x} \leq b\right) \leq \delta \Longleftrightarrow \mathbf{a}^{T} \hat{\mathbf{x}}-b \geq c
$$
where $c=\operatorname{erf}^{-1}(1-2 \delta) \sqrt{2 \mathbf{a}^{T} \Sigma \mathbf{a}}$.
Gaussian distribution:
- pdf: $f(x|\mu,\sigma^2)=\frac{1}{\sqrt{2\pi \sigma^2}}e^{-\frac{(x-\mu)^2}{2\sigma^2}}$
- cdf: $\Phi(x) = \frac{1}{\sqrt{2\pi}} \int\limits_{-\inf}^x e^{- \frac{t^2}{2}} dt$
- erf: $\operatorname{erf}(x) = \frac{2}{\sqrt{\pi}} \int\limits_0^x e^{-t^2} dt$ (gives the probability of a random variable with normal distribution of mean 0 and variance 1/2 falling in the range $[-x,x]$)
- Gaussian form translation: $f(x|\mu,\sigma^2)=\frac{1}{\sigma} \varphi(\frac{x-\mu}{\sigma})$
Inter-robot Collision Avoidance Constraint

Two robot position $\mathbf{p}{i} \sim \mathcal{N}\left(\hat{\mathbf{p}}{i}, \Sigma_{i}\right)$, $\mathbf{p}{j} \sim \mathcal{N}\left(\hat{\mathbf{p}}{j}, \Sigma_{j}\right)$, the collision probability is
$$
\operatorname{Pr}\left(\mathbf{x}{i} \in C{i j}\right)=\int_{\mathbb{R}^{3}} I_{C}\left(\mathbf{p}{i}, \mathbf{p}{j}\right) p\left(\mathbf{p}{i}\right) p\left(\mathbf{p}{j}\right) d \mathbf{p}{i} d \mathbf{p}{j}
$$
where $I_C$ is the indicator function, i.e.,
$$
I_{C}\left(\mathbf{p}{i}, \mathbf{p}{j}\right)=\left{\begin{array}{ll}{1,} & {\text { if }\left|\mathbf{p}{i}-\mathbf{p}{j}\right| \leq r_{i}+r_{j}} \ {0,} & {\text { otherwise }}\end{array}\right.
$$

Assume $\mathbf{p}{i}$ and $\mathbf{p}{j}$ independent. $\mathbf{p}{i}-\mathbf{p}{j}\sim \mathcal{N}\left({\mathbf{p}{i}}-{\mathbf{p}{j}}, \Sigma_{i}-\Sigma_{j} \right)$, so
$$
\operatorname{Pr}\left(\mathbf{x}{i} \in C{i j}\right)=\int_{\left|\mathbf{p}{i}-\mathbf{p}{j}\right| \leq r_{i}+r_{j}} p\left(\mathbf{p}{i}-\mathbf{p}{j}\right) d\left(\mathbf{p}{i}-\mathbf{p}{j}\right).
$$

Here, Collision region is a sphere, and the distribution of $\mathbf{p}_{ij}$ is a ellipsoid. Hard to compute.

Transform the spherical collision region $C_{ij}$ into half space $\tilde{C}_{ij}$
$$
\tilde{C}{i j} :=\left{\mathbf{x} | \mathbf{a}{i j}^{T}\left(\mathbf{p}{i}-\mathbf{p}{j}\right) \leq b_{i j}\right}
$$
where $\mathbf{a}{i j}=\left(\hat{\mathbf{p}}{i}-\hat{\mathbf{p}}{j}\right) /\left|\hat{\mathbf{p}}{i}-\hat{\mathbf{p}}{j}\right| \text { and } b{i j}=r_{i}+r_{j}$

Because $C_{ij} \subset \tilde{C}{ij}$, $\operatorname{Pr}\left(\mathbf{x}{i} \in C_{i j}\right) \leq \operatorname{Pr}\left(\mathbf{x}{i} \in \tilde{C}{i j}\right)$.

Based on Lemma 1, an upper bound can be obtained:
$$
\operatorname{Pr}\left(\mathbf{x}{i} \in C{i j}\right) \leq \frac{1}{2}+\frac{1}{2} \operatorname{erf}\left(\frac{b_{i j}-\mathbf{a}{i j}^{T}\left(\hat{\mathbf{p}}{i}-\hat{\mathbf{p}}{j}\right)}{\sqrt{2 \mathbf{a}{i j}^{T}\left(\Sigma_{i}+\Sigma_{j}\right) \mathbf{a}{i j}}}\right)
$$
Based on Lemma 2, transform the chance constraint into deterministic constraint:
$$
\mathbf{a}{i j}^{T}\left(\hat{\mathbf{p}}{i}-\hat{\mathbf{p}}{j}\right)-b_{i j} \geq \operatorname{erf}^{-1}\left(1-2 \delta_{r}\right) \sqrt{2 \mathbf{a}{i j}^{T}\left(\Sigma{i}+\Sigma_{j}\right) \mathbf{a}_{i j}}
$$
Robot-Obstacle Collision Chance constraint

position of robots and obstacles are independent.
$$
\operatorname{Pr}\left(\mathbf{x}{i} \in C{i o}\right)=\int_{\left|\mathbf{p}{i}-\mathbf{p}{o}\right|{\Omega{i o}} \leq 1} p\left(\mathbf{p}{i}-\mathbf{p}{o}\right) d\left(\mathbf{p}{i}-\mathbf{p}{o}\right)
$$
Here, collision region is ellipsoid.

Linearize the collision

Affine coordinate transformation $\tilde{\mathbf{y}}=\Omega_{jo}^{\frac{1}{2}} \mathbf{y}$ to transform ellipsoid to unit sphere $\tilde{C}_{io}$. Accordingly, the robot and obstacel position need to be transformed into new distribution

$\tilde{\mathbf{p}}{i} \sim \mathcal{N}\left(\hat{\tilde{\mathbf{p}}}{i}, \tilde{\Sigma}{i}\right)$, $\tilde{\mathbf{p}}{o} \sim \mathcal{N}\left(\hat{\mathbf{p}}{o}, \tilde{\Sigma}{o}\right)$, where
$$
\begin{aligned} \hat{\mathbf{p}}{i}=\Omega{i o}^{\frac{1}{2}} \hat{\mathbf{p}}{i}, & \tilde{\Sigma}{i}=\Omega_{i o}^{\frac{1}{2} T} \Sigma_{i} \Omega_{i o}^{\frac{1}{2}} \ \hat{\hat{\mathbf{p}}}{o}=\Omega{i o}^{\frac{1}{2}} \hat{\mathbf{p}}{o}, & \tilde{\Sigma}{o}=\Omega_{i o}^{\frac{1}{2} T} \Sigma_{o} \Omega_{i o}^{\frac{1}{2}} \end{aligned}
$$
let $\operatorname{Pr}\left(\tilde{\mathbf{x}}{i} \in \tilde{C}{i o}\right)=\int_{\left|\mathbb{D}{i}-\tilde{\mathbf{p}}{o}\right| \leq 1} p\left(\tilde{\mathbf{p}}{i}-\tilde{\mathbf{p}}{o}\right) d\left(\tilde{\mathbf{p}}{i}-\tilde{\mathbf{p}}{o}\right) $, we have
$$
\operatorname{Pr}\left(\mathbf{x}{i} \in C{i o}\right)=\operatorname{Pr}\left(\tilde{\mathbf{x}}{i} \in \tilde{C}{i o}\right)
$$
Use same linearization method for phere with $\mathbf{a}{i o}=\left(\hat{\mathbf{p}}{i}-\hat{\mathbf{p}}{o}\right) /\left|\hat{\mathbf{p}}{i}-\hat{\mathbf{p}}{o}\right| \text { and } b{i o}=1$.

Transform into deterministic constraint:
$$
\mathbf{a}{i o}^{T} \Omega{i o}^{\frac{1}{2}}\left(\hat{\mathbf{p}}{i}-\hat{\mathbf{p}}{j}\right)- b_{i o} \geq \operatorname{erf}^{-1}\left(1-2 \delta_{o}\right) \cdot \sqrt{2 \mathbf{a}{i o}^{T} \Omega{i o}^{\frac{1}{2}}\left(\Sigma_{i}+\Sigma_{j}\right) \Omega_{i o}^{\frac{1}{2} T} \mathbf{a}_{i o}}
$$

Deterministic MPC Formulation

Deterministic MPC formulation

Cost function
- terminal cost:
- stage cost
- potential field cost : increase the separation between robots and obstacles
  $$
  J_{i, c}^{k}\left(\hat{\mathbf{x}}{i}^{k}\right)=\sum{j \in \mathcal{I} \cup \mathcal{I}{o}, j \neq i} {J{i, j, c}^{k}(\hat{\mathbf{x}}{i}^{k}})
  $$
  With
  $$
  J{i, j, c}^{k}\left(\hat{\mathbf{x}}{i}^{k}\right)=
  \begin{cases}
  l{i, c}^{k}\left(d_{i j}^{\text {safe }}-d_{i j}\right), & d_{i j}<d_{i j}^{\text {safe }} \
  0, & d_{i j}\ge d_{i j}^{\text {safe }}
  \end{cases}
  $$
Constraints:
- transformed into deterministic constraints
Problem:
$$
\begin{array}{c}{\min {\hat{\mathbf{x}}{i}^{1 : N}, \mathbf{u}{i}^{0 : N-1}} \quad J{i}^{N}\left(\hat{\mathbf{x}}{i}^{N}\right)+\sum{k=0}^{N-1} J_{i, u}^{k}\left(\mathbf{u}{i}^{k}\right)+\sum{k=1}^{N} J_{i, c}^{k}\left(\hat{\mathbf{x}}{i}^{k}\right)} \ {\text { s.t. } \quad \mathbf{x}{i}^{0}=\hat{\mathbf{x}}{i}(0), \quad \hat{\mathbf{x}}{i}^{k}=\mathbf{f}{i}\left(\hat{\mathbf{x}}{i}^{k-1}, \mathbf{u}{i}^{k-1}\right)} \ {g{i j}^{k}\left(\hat{\mathbf{x}}{i}^{k}, \hat{\mathbf{p}}{j}^{k}, \Sigma_{i}^{k}, \Sigma_{j}^{k}, \delta_{r}\right) \leq 0} \ {g_{i o}^{k}\left(\hat{\mathbf{x}}{i}^{k}, \hat{\mathbf{p}}{o}^{k}, \Sigma_{i}^{k}, \Sigma_{o}^{k}, \delta_{o}\right) \leq 0} \ {\mathbf{u}{i}^{k-1} \in \mathcal{U}{i}, \quad \hat{\mathbf{x}}{i}^{k} \in \mathcal{X}{i}} \ {\forall j \neq i \in \mathcal{I} ; \forall o \in \mathcal{I}{o} ; \forall k \in{1, \ldots, N}}\end{array}
$$
where $g{ij}$ and $g_{io}$ are deterministic constraints.

How to obtain the mean and covariance of position

Constant velocity model without communication
Sequential Planning with communication
Distributed planning with communication