S形曲線速度規劃--5段式

爲了減小機器人加速過程的衝擊，這裏梳理一下S形加減速相關知識。

1. S加減速曲線

計算公式：
加加速度

$j(t)=\begin{cases} J & 0\leq t \leq t_1\\ -J & t_1\leq t \leq t_2\\ 0 & t_2\leq t \leq t_3\\ -J & t_3\leq t \leq t_4\\ J & t_4\leq t \leq t_5\\ \end{cases}$

加速度：

$a(t)=\begin{cases} J*t & 0\leq t \leq t_1\\ J*T_1 - J*(t-t_1) & t_1\leq t \leq t_2\\ 0 & t_2\leq t \leq t_3\\ -J*(t-t_3) & t_3\leq t \leq t_4\\ -J*T_4 + J*(t-t_4) & t_4\leq t \leq t_5\\ \end{cases}$

速度：

$v(t)=\begin{cases} v_s + J*t^2/2 & 0\leq t \leq t_1\\ v_1 + J*T_1*(t-t_1) - J*(t-t_1)^2/2 & t_1\leq t \leq t_2\\ v_2 & t_2\leq t \leq t_3\\ v_3 - J*(t-t_3)^2/2 & t_3\leq t \leq t_4\\ v_4 - J*T_4*(t-t_4) + J*(t-t_4)^2/2 & t_4\leq t \leq t_5\\ \end{cases}$

位移：

​
$s(t)=\begin{cases} v_s*t + J*t^3/6 & 0\leq t \leq t_1\\ s_1 + v_1*(t-t_1) + J*T_1*(t-t_1)^2/2 - J*(t-t_1)^3/6 & t_1\leq t \leq t_2\\ s_2 + v_2*(t-t_2) & t_2\leq t \leq t_3\\ s_3 + v_3*(t-t_3) - J*(t-t_3)^3/6 & t_3\leq t \leq t_4\\ s_4 + v_4*(t-t_4) - J*T_4*(t-t_4)^2/2 + J*(t-t_4)^3/6 & t_4\leq t \leq t_5\\ \end{cases}$

2. 算法流程（vs=0, ve=0）

已知 位移S，最大速度V 加加速度J，按照如下流程進行速度規劃：

第一步，計算加速減速距離：

$S_a = S_d = V*\sqrt\frac{V}{J}$

第二步，比較位移關係

$如果 S_a + S_d <= S \\ 則：\\ t_1 = t_2 = t_4 = t_5 = \sqrt\frac{V}{J}\\ t_3 = \frac{S-S_a-S_d}{V}$

$如果 S_a + S_d > S \\ 則：\\ V^, = (\frac{S*J}{2})^\frac{1}{3} \\ t_1 = t_2 = t_4 = t_5 = \sqrt\frac{V^,}{J}\\ t_3 = 0$

第三步，即得到每階段對應的插補時間，即可進行速度插補

3. 算法流程（vs>0或 ve>0）

已知 位移S，起始速度Vs,終止速度Ve,最大速度V 加加速度J，按照如下流程進行速度規劃：

第一步，計算加速減速距離：

$S_a = (V+V_s)*\sqrt\frac{V-V_s}{J} \\ S_d = (V+V_e)*\sqrt\frac{V-V_e}{J}$

第二步，比較位移關係

$如果 S_a + S_d <= S \\ 則：\\ t_1 = t_2 = \sqrt\frac{V-V_s}{J}\\ t_4 = t_5 = \sqrt\frac{V-V_e}{J}\\ t_3 = \frac{S-S_a-S_d}{V}$

如果 	S_a + S_d > S 
重新計算最大速度V,因爲解析解計算複雜，這裏通過二分法逼近，方法如下：

(1)設置v1 = V,v2設置爲Vs,Ve中較大的數值

(2)二分法計算最大速度

$V = \frac{v1+v2}{2}$

(3)計算加減速距離

$S_a = (V+V_s)*\sqrt\frac{V-V_s}{J} \\ S_d = (V+V_e)*\sqrt\frac{V-V_e}{J}$

(3)比較位移

$如果 S > S_a + S_d \\ 則：\\ v2 = V 如果 S < S_a + S_d \\ 則：\\ v1 = V$

(4) 重複步驟(1)(2)(3),直到

$|S-S_a-S_d| < \epsilon$

閾值ε根據實際需求進行設置，通常設置爲0.000001，迭代次數不超過10次.