# Graph-Based planning

## Introduction
Graph-based algorithm, like Dijkstra, A*, based on waypoints created on the graph the machine/robot uses.
However, to make feasible motion connections, the graph needs some adjustment.

## Previous Work
- [Dubbins 1957](https://sci-hub.se/10.2307/2372560)
- [Reeds-Shepp 1990](http://sector3.imm.uran.ru/shepp/Reeds_Shepp_trunk.pdf)
- [State-Lattice]()
- [Curvature]()

### Dubbins
[感謝前人的筆記證明](https://zhuanlan.zhihu.com/p/414753861)
這邊的圖片筆記都是從他那邊來的
#### Overview
$R = \cfrac{L}{\tan{\delta_f}}, \text{L 為前軸到後軸的長度}\delta_f \text{為最大前輪轉角}$
==滿足motion connections==, 可令$\underline{\delta = vT_s}$
$x_{new} = x_{prev} + \delta * \cos{\theta}\\
y_{new} = y_{prev} + \delta * \sin{\theta}\\
\theta_{new} = \theta_{prev} + \frac{\delta}{R}$
L: turn left
S: straight
R: turn right

LSL:

$p\cos{(\alpha + t)} - \sin{\alpha} + \sin{\beta} = d\\
p\sin{(\alpha + t)} + \cos{\alpha} - \cos{\beta} = 0\\
\alpha + t + q = \beta \% 2 \pi$
共有六種組合，上面連結跟[paper](https://sci-hub.se/10.2307/2372560)都有證明
==在無障礙空間運行==
### Reeds-Shepp (RS)
Dubins的衍生，由原本6種，增加到48種(考慮前後: $6 * 2 * 2$)，對於motion的表示法做出一點點修改
$\theta = u_1u_2, u_1 \in {-1, 1}$, 表前後移動
利用時間對稱性及reflect,組出48種組合
[可以參考這篇的說明](https://zhuanlan.zhihu.com/p/122544884)
RS 在 [Hybrid A*](https://ai.stanford.edu/~ddolgov/papers/dolgov_gpp_stair08.pdf)的應用:
每N個node做一次產生不同的collision-free的RS path. 類似像投影片的tree-based method的做法 (我猜RS會有點像A* 裡的heuristic)
### BVP
https://zhuanlan.zhihu.com/p/150698376
[補](https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=7299672&tag=1)

### State Lattice
簡單來說就是另一種Heuristic,
利用上面的BVP，求得cost function $J = \gamma^2 + \beta\gamma T + \frac{1}{3} \beta^2T^2 + \frac{1}{3}\alpha \gamma T^2 + \frac{1}{4}\alpha \beta T^3 + \frac{1}{20} \alpha^2 T^4$
會使用State lattice 應該是因為它可以**optimize**而且理論上可以做到全局規劃

### 最後一篇Grid-based + topological map的還沒讀
[這邊是連結](https://www.ri.cmu.edu/pub_files/pub1/thrun_sebastian_1996_1/thrun_sebastian_1996_1.pdf)

### Reference
[RS](https://blog.csdn.net/robinvista/article/details/95137143)
graph: 用 State Lattice
- [簡單說明這是啥](https://zhuanlan.zhihu.com/p/403822524)
- 
- [跟5次式有關的東西](https://zhuanlan.zhihu.com/p/145466792)
- [很厲害的酷東西](https://www.ri.cmu.edu/pub_files/pub1/thrun_sebastian_1996_1/thrun_sebastian_1996_1.pdf)