---
tags: Robot id, Overall, Motion Planning
---

Motion PLanning
===

## <font size = 5>Scurve</font>

<img src =https://i.imgur.com/XGCsMcc.png class = "ph_8">

### Acceleration
<img src = https://i.imgur.com/Yo4L1I0.png class = "ph_6">

各時間點加速度

| Time | Acceleration |
|:----------- | ---------------------------- |
| $t_1>t$ | $a(t)=Kt$ |
| $t_2≥t>t_1$ | $a(t)=Kt_1$ |
| $t_3≥t>t_2$ | $a(t)=Kt_1-K(t-t_2 )$ |
| $t_4≥t>t_3$ | $a(t)=0$ |
| $t_5≥t>t_4$ | $a(t)=-K(t-t_4 )$ |
| $t_6≥t>t_5$ | $a(t)=-K(t_5-t_4)$ |
| $t_7≥t>t_6$ | $a(t)=-K(t_5-t_4 )+K(t-t_6)$ |

各時間點加速度
- $a(t_1 )=Kt_1$
- $a(t_2)=Kt_1$
- $a(t_3 )=0$
- $a(t_4 )=0$
- $a(t_5 )=-K(t_5-t_4)$
- $a(t_6 )=-K(t_5-t_4)$
- $a(t_7 )=-K(t_5-t_4 )+K(t_7-t_6)$

### Velocity
| Time | Velocity |
|:----------- | ---------------------------- |
| $t_1>t$ | $v(t)=1/2 Kt^2$ |
| $t_2≥t>t_1$ | $v(t)=-1/2 Kt_1^2+Kt_1 t$ |
| $t_3≥t>t_2$ | $v(t)=-1/2 Kt_1^2-1/2 Kt_2^2+Kt*t_2+Kt_1 t-1/2 Kt^2$ |
| $t_4≥t>t_3$ | $v(t)=-1/2 Kt_1^2-1/2 Kt_2^2-1/2 Kt_3^2+Kt_1 t_3+Kt_2 t_3$ |
| $t_5≥t>t_4$ | $v(t)=-1/2 Kt_1^2-1/2 Kt_2^2-1/2 Kt_3^2-1/2 Kt_4^2$$+Kt_1 t_3+Kt_2 t_3+Kt*t_4-1/2 Kt^2$ |
| $t_6≥t>t_5$ | $v(t)=-1/2 Kt_1^2-1/2 Kt_2^2-1/2 Kt_3^2-1/2 Kt_4^2+1/2 Kt_5^2$$+Kt_1 t_3+Kt_2 t_3-K(t_5-t_4)t$ |
| $t_7≥t>t_6$ | $v(t)=-1/2 Kt_1^2-1/2 Kt_2^2-1/2 Kt_3^2-1/2 Kt_4^2+1/2 Kt_5^2$$+1/2 Kt_6^2+Kt_1 t_3+Kt_2 t_3-K(t_5-t_4 )*t-K*t_6*t+1/2 Kt^2$ |

各時間點加速度
- $v(t_1 )=1/2 Kt_1^2$
- $v(t_2 )=-1/2 Kt_1^2+Kt_1 t_2$
- $v(t_3 )=-1/2 Kt_1^2-1/2 Kt_2^2-1/2 Kt_3^2+Kt_1 t_3+Kt_2 t_3$
- $v(t_4 )=-1/2 Kt_1^2-1/2 Kt_2^2-1/2 Kt_3^2+Kt_1 t_3+Kt_2 t_3$
- $v(t_5 )=-1/2 Kt_1^2-1/2 Kt_2^2-1/2 Kt_3^2-1/2 Kt_4^2-1/2 Kt_5^2+Kt_1 t_3+Kt_2 t_3+Kt_4 t_5$
- $v(t_6 )=-1/2 Kt_1^2-1/2 Kt_2^2-1/2 Kt_3^2-1/2 Kt_4^2+1/2 Kt_5^2+Kt_1 t_3+Kt_2 t_3+Kt_4 t_6-Kt_5 t_6$
- $v(t_7 )=-1/2 Kt_1^2-1/2 Kt_2^2-1/2 Kt_3^2-1/2 Kt_4^2+1/2 Kt_5^2+1/2 Kt_6^2+1/2 Kt_7^2+Kt_1 t_3$$+Kt_2 t_3+Kt_4 t_7-Kt_5 t_7-Kt_6 t_7$

Position
| Time | Position |
|:----------- | ---------------------------- |
|$t_1≥t$|$s(t)=1/6 Kt^3$|
|$t_2≥t>t_1$|$s(t)=1/6 Kt_1^3-1/2 Kt_1^2*t+1/2 Kt_1 t^2$|
|$t_3≥t>t_2$|$s(t)=1/6 Kt_1^3+1/6 Kt_2^3-(1/2 Kt_1^2+1/2 Kt_2^2 )*t+1/2 K(t_2+t_1 )*t^2$$-1/6 Kt^3$|
|$t_4≥t>t_3$|$s(t)=1/6 Kt_1^3+1/6 Kt_2^3+1/3 Kt_3^3-1/2 Kt_1 t_3^2-1/2 Kt_2 t_3^2+(-1/2 Kt_1^2-1/2 Kt_2^2-1/2 Kt_3^2+Kt_1 t_3+Kt_2 t_3)t$|
|$t_5≥t>t_4$|$s(t)=1/6 Kt_1^3+1/6 Kt_2^3+1/3 Kt_3^3+1/6 Kt_4^3-1/2 Kt_1 t_3^2-1/2 Kt_2 t_3^2+(-1/2 Kt_1^2-1/2 Kt_2^2-1/2 Kt_3^2-1/2 Kt_4^2+Kt_1 t_3+Kt_2 t_3 )*t+1/2 Kt_4*t^2-1/6 Kt^3$|
|$t_6≥t>t_5$|$s(t)=1/6 Kt_1^3+1/6 Kt_2^3+1/3 Kt_3^3+1/6 Kt_4^3-1/6 Kt_5^3-1/2 Kt_1 t_3^2-1/2 Kt_2 t_3^2+(-1/2 Kt_1^2-1/2 Kt_2^2-1/2 Kt_3^2-1/2 Kt_4^2+1/2 Kt_5^2+Kt_1 t_3+Kt_2 t_3)t-1/2 K(t_5-t_4 ) t^2$|
|$t_7≥t>t_6$|$s(t)=1/6 Kt_1^3+1/6 Kt_2^3+1/3 Kt_3^3+1/6 Kt_4^3-1/6 Kt_5^3-1/2 Kt_1 t_3^2-1/6 Kt_6^3-1/2 Kt_2 t_3^2+(-1/2 Kt_1^2-1/2 Kt_2^2-1/2 Kt_3^2-1/2 Kt_4^2+1/2 Kt_5^2+1/2 Kt_6^2+Kt_1 t_3+Kt_2 t_3)t-K/2 (t_5-t_4+t_6 )*t^2+1/6 Kt^3$|

各時間點位置
- $s(t_1 )=1/6 Kt_1^3$
- $s(t_2 )=1/6 Kt_1^3-1/2 Kt_1^2 t_2+1/2 Kt_1 t_2^2$
- $s(t_3 )=1/6 Kt_1^3+1/6 Kt_2^3-1/6 Kt_3^3-1/2 Kt_1^2 t_3+1/2 Kt_1 t_3^2-1/2 Kt_2^2 t_3+1/2 Kt_2 t_3^2$
- $s(t_4)=1/6 Kt_1^3+1/6 Kt_2^3+1/3 Kt_3^3-1/2 Kt_1 t_3^2-1/2 Kt_1^2 t_4-1/2 Kt_2 t_3^2-1/2 Kt_2^2 t_4$$-1/2 Kt_3^2 t_4+Kt_1 t_3 t_4+Kt_2 t_3 t_4$
- $s(t_5 )=1/6 Kt_1^3+1/6 Kt_2^3+1/3 Kt_3^3+1/6 Kt_4^3-1/6 Kt_5^3-1/2 Kt_1 t_3^2-1/2 Kt_1^2 t_5$$-1/2 Kt_2 t_3^2-1/2 Kt_2^2 t_5-1/2 Kt_3^2 t_5-1/2 Kt_4^2 t_5+1/2 Kt_4 t_5^2+Kt_1 t_3 t_5+Kt_2 t_3 t_5$
- $s(t_6 )=1/6 Kt_1^3+1/6 Kt_2^3+1/3 Kt_3^3+1/6 Kt_4^3-1/6 Kt_5^3-1/2 Kt_1 t_3^2-1/2 Kt_1^2 t_6$$-1/2 Kt_2 t_3^2-1/2 Kt_2^2 t_6-1/2 Kt_3^2 t_6-1/2 Kt_4^2 t_6-1/2 Kt_5 t_6^2+1/2 Kt_5^2 t_6+Kt_1 t_3 t_6$$+Kt_2 t_3 t_6+1/2 Kt_4 t_6^2$
- $s(t_7 )=1/6 Kt_1^3+1/6 Kt_2^3+1/3 Kt_3^3+1/6 Kt_4^3-1/6 Kt_5^3-1/6 Kt_6^3+1/6 Kt_7^3$$-1/2 Kt_1 t_3^2-1/2 Kt_1^2 t_7+Kt_1 t_3 t_7-1/2 Kt_2 t_3^2-1/2 Kt_2^2 t_7+Kt_2 t_3 t_7-1/2 Kt_3^2 t_7$$-1/2 Kt_4^2 t_7+1/2 Kt_4 t_7^2+1/2 Kt_5^2 t_7- 1/2 Kt_5 t_7^2+1/2 Kt_6^2 t_7-1/2 Kt_6 t_7^2$