reveal.js

Multi-Robot Collision Avoidance

Motivation

Multi-Robot affects most of these!

Physics: Differential Drive Robot

Demo

## Robot Controller
						- Assume we have desired state `$x_d, y_d, \theta_d$` and desired `$v_d, \omega_d$`
						- controller has a feedforward term and feedback term [(Reference)](https://doi.org/10.1109/ROBOT.1990.126006)
						- `$K_x, K_y, K_\theta\in\mathbb R^+$` are tuning gains

<!-- 5.10 in https://web2.qatar.cmu.edu/~gdicaro/16311-Fall17/slides/control-theory-for-robotics.pdf

`$$\begin{aligned}
						v_{ctrl} &= v_d \cos(\theta_d - \theta) + k_1 ((x_d - x)\cos (\theta) + (y_d-y)\sin (\theta))\\
						\omega_{ctrl} &= \omega_d + k_2 \operatorname{sign}(s_d) ((x_d-x)\cos (\theta) - (y_d-y)\sin (\theta)) + k_3 (\theta_d - \theta)
						\end{aligned}$$`
						-->

<!-- 10.1109/ROBOT.1990.126006, (8) and (4)
						
							v_ctrl = v_r \cos \theta_e + K_x x_e
							omega_ctrl = omega_r + v_r (K_y y_e + K_\theta \sin \theta_e)

-->

`$$\begin{aligned}
						x_e &= (x_d-x)\cos \theta + (y_d-y)\sin \theta\\
						y_e &= -(x_d-x)\sin \theta + (y_d-y)\cos \theta\\
						\theta_e &= \theta_d - \theta \\
						v_{ctrl} &= v_d \cos \theta_e + K_x x_e\\
						\omega_{ctrl} &= \omega_d + v_d (K_y y_e + K_\theta \sin \theta_e)
						\end{aligned}$$`

---
						## Demo

---
						## Differential Flatness

- Find a *mapping* from workspace to state space
						- If we have a desired 2D smooth curve `$p(t)$` (e.g., polynomial), can we compute desired states?

`$$\begin{aligned}
							\frac{\dot y}{\dot x} &= \frac{\frac{r}{2} (u_l + u_r) \sin \theta}{\frac{r}{2} (u_l + u_r) \cos \theta}\\
							\frac{\dot y}{\dot x} &= \frac{\sin \theta}{\cos \theta} = \tan\theta\\
							\Rightarrow \theta &= \arctan \frac{\dot y}{\dot x}
						\end{aligned}$$`

---
						## Differential Flatness
						`$$\begin{aligned}
						\omega &= \dot \theta = \frac{d}{dt} \arctan \left(\frac{\dot y}{\dot x}\right)\\
						&= \frac{\dot x \ddot y - \dot y \ddot x}{\dot x^2 + \dot y^2}
						\end{aligned}$$`

`$$\begin{aligned}
						s &= \frac{\dot x}{\cos \theta}\\
							&= \frac{\dot x}{\cos \left(\arctan \left(\frac{\dot y}{\dot x}\right)\right)}\\
							&= \dot x \sqrt{\frac{\dot y^2}{\dot x^2} + 1} = \dot x \sqrt{\frac{\dot y^2}{\dot x^2} + \frac{\dot x^2}{\dot x^2}}\\
							&= {\color{red} \pm}\sqrt{\dot y^2 + \dot x^2}
						\end{aligned}$$`

---
						## Demo

Bézier Curves

Bézier Curve

A Bézier curve $\vp : [0, 1] \rightarrow \mathbb R^d$ of degree $n$ is defined by $n+1$ control points $\vp_0, \ldots, \vp_n \in \mathbb R^d$ as follows: $$\begin{align*} \vp(t) &= \sum_{i=0}^n b_{i,n}(t) \vp_i\\ b_{i,n}(t) &= {n \choose i} t^i\left(1-t\right)^{n-i}. \end{align*}$$

Cubic Bézier Curve

$$\begin{align*} \vp(t) &= (1-t)^3 \vp_0 + 3 t (1-t)^2 \vp_1 \\ &+ 3 t^2 (1-t) \vp_2 + t^3 \vp_3 \end{align*}$$

Bézier Curves Properties

Endpoint interpolation: The curve connects $\vp_0$ and $\vp_n$, i.e., $\vp(0) = \vp_0$ and $\vp(1) = \vp_n$
$C^n$ smoothness
Convex hull property: The curve lies inside the convex hull of their control points, i.e., $\vp(t) \in ConvexHull\{\vp_0,\ldots,\vp_n\}\ \forall t\in[0, 1]$

Cubic Bézier Curve Optimization

Cubic Bézier Curve

$$\begin{align*} \vp(t) &= (1-t)^3 \vp_0 + 3 t (1-t)^2 \vp_1 + 3 t^2 (1-t) \vp_2 + t^3 \vp_3\\ \vp(0) &= \vp_0\\ \vp(1) &= \vp_3\\ \dot{\vp}(t) &= 3(1-t)^2 (\vp_1 - \vp_0) + 6 (1-t) t (\vp_2 - \vp_1) + 3t^2 (\vp_3 - \vp_2)\\ \dot \vp(0) &= 3(\vp_1 - \vp_0)\\ \dot \vp(1) &= 3(\vp_3 - \vp_2)\\ \end{align*}$$

Demo

Voronoi Cells

Voronoi region

Let $q_1,\ldots,q_K$ be a set of configurations on the state space $\mathcal Q$. The Voronoi region is defined as $$ R_k = \{ q \in \mathcal Q\mid d(q, R_k) \leq d(q, R_j), \text{ for all } j \neq k\} $$

Collision Avoidance Via Buffered Voronoi Cells (BVC)

Sense neighbor robots' position
Compute Voronoi regions (safe polyhedra per robot)
Shrink ("buffer") regions to account for robot size (still polyhedra)
Trajectory optimization / control with constraint to stay within safe region for some time

D. Zhou, Z. Wang, S. Bandyopadhyay, and M. Schwager, “Fast, on-line collision avoidance for dynamic vehicles using buffered voronoi cells,” IEEE Robotics and Automation Letters (RA-L), vol. 2, no. 2, pp. 1047–1054, 2017

Multi-Robot Collision Avoidance

Motivation

Physics: Differential Drive Robot

Physics: Differential Drive Robot

Physics: Differential Drive Robot

Demo

Bézier Curves

Bézier Curves Properties

Cubic Bézier Curve Optimization

Demo

Voronoi Cells

Collision Avoidance Via Buffered Voronoi Cells (BVC)

Demo

Try it Yourself