Motion planning
as search II
MIT 6.832: Underactuated Robotics
Spring 2022, Lecture 20
Follow live at https://slides.com/d/ubLxnxU/live
(or later at https://slides.com/russtedrake/spring22-lec20)
Image credit: Boston Dynamics
Motion Planning around Obstacles with Convex Optimization
Tobia Marcucci*, Mark Petersen*, David von Wrangel, Russ Tedrake*. To appear on arxiv any day now.
Shortest Paths on Graphs of Convex Sets
Graphs of Convex Sets
-
For each \(i \in V:\)
- Compact convex set \(X_i \subset \R^d\)
- A point \(x_i \in X_i \)
- Edge length given by a convex function \[ \ell(x_i, x_j) \]
New shortest path formulation
\begin{aligned} \min_{\varphi} \quad & \sum_{(i,j) \in E} c_{ij} \varphi_{ij} \\
\mathrm{s.t.} \quad
& \sum_{j \in E_i^{out}} \varphi_{ij} + \delta_{ti} = \sum_{j \in E_i^{in}} \varphi_{ji} + \delta_{si}, && \forall i \in V, \\
& \varphi_{ij} \geq 0, && \forall (i,j) \in E.
\end{aligned}
Classic shortest path LP
\begin{aligned} \min_{\varphi,x} \quad & \sum_{(i,j) \in E} \ell_{ij}(x_i, x_j) \varphi_{ij} \\
\mathrm{s.t.} \quad
& x_i \in X_i, && \forall i \in V, \\
& \sum_{j \in E_i^{out}} \varphi_{ij} + \delta_{ti} = \sum_{j \in E_i^{in}} \varphi_{ji} + \delta_{si} \le 1, && \forall i \in V, \\
& \varphi_{ij} \geq 0, && \forall (i,j) \in E.
\end{aligned}
now w/ Convex Sets
New shortest path formulation
\begin{aligned} \min_{\varphi,x} \quad & \sum_{(i,j) \in E} \ell_{ij}(x_i, x_j) \varphi_{ij} \\
\mathrm{s.t.} \quad
& x_i \in X_i, && \forall i \in V, \\
& \sum_{j \in E_i^{out}} \varphi_{ij} + \delta_{ti} = \sum_{j \in E_i^{in}} \varphi_{ji} + \delta_{si} \le 1, && \forall i \in V, \\
& \varphi_{ij} \geq 0, && \forall (i,j) \in E.
\end{aligned}
- Use convex hull reformulation + perspective functions to rewrite this as mixed-integer convex.
- Strengthen convex relaxation by adding additional convex constraints (implied at binary feasibility).
minimum distance
minimum time
IRIS (Fast approximate convex segmentation)
- Iteration between (large-scale) quadratic program and (relatively compact) semi-definite program (SDP)
- Scales to high dimensions, millions of obstacles
- ... enough to work on raw sensor data
Graph of Convex Sets (GCS)
PRM
PRM w/ short-cutting