S形曲線加減速算法流程--7段式

7段S形加減速的算法流程.
7段S形加減速相對於5段，增加了勻加速過程，計算流程更加複雜一些，需要進行多次分類討論。

加減速規劃的流程：

曲線加減速規劃是否存在勻速段取決於速度達到給定的指令速度口時，加速段和減速段位移之和s'是否小於待插補位移s．
加速及減速過程中是否存在勻加速段或勻減速段則由vs,ve,v,a,j決定。

規劃後的S曲線可能出現下圖中8中情況：

1.勻加速段存在的條件

若：

$v-v_s > a^2_m/j_m$

則：

$\begin{cases} t_1 & = a_m/j_m \\ t_2 & = (v-v_s)/a_m-t_1 \\ t_3 & = t_1 \\ \end{cases}$

若：

$v-v_s \ge a^2_m/j_m$

則：

$\begin{cases} &t_1 = \sqrt{(v-v_s)/j_m} \\ &t_2 = 0 \\ &t_3 = t_1\\ &a_m = \sqrt{(v-v_s)/j_m} \end{cases}$

使用上述計算得到的參數，計算加速段位移，記爲：

$S_{ac}$

按照相同的過程，可以計算減速段位移，記爲：

$S_{de}$

2.勻速段存在的條件

若：

$S_{ac} + S_{de} < S$

則：

$t_4 = (S - S_{ac} - S_{de})/v \\$

若：

$S_{ac} + S_{de} \ge S$

則：

$t_4 = 0\\ 實際能達到的最大速度小於v$

3.不存在勻速段

假設：

$v_{max} = max(v_s,v_e)+a^2_m/j_m$

$取v_s，v_e中較大的速度計算，帶入步驟1中，得到加減速總位移S_m$

若：

$S_m < S$

則：

$實際最大速度:\\ v^{'} = -a^2_m/(2*j_m)+\sqrt{\frac{a^4_m-2*j_m*[a^2_m*(v_s+v_e)-j_m*(v^2_s+v^2_e)-2a_m*j_m*S]}{2*j_m}}$

若：

$S_m = S$

$則：實際最大速度就是v = v^{'}$

若：

$S_m > S$

勻加速段和勻減速段不同時存在，需要重新計算最大速度，
假設：

$v^{''} = min(v_s,v_e)+a^2_m/j_m\\ 繼續根據此最大速度計算加減速總位移Sm$

若：

$S_m = S 實際最大速度v = v^{''}$

若：

$S_m < S$

則實際只有勻加速段或者勻減速段，需重新計算最大速度，這種情況使用二分法，參考本博客中5段S形加減速中的方法計算即可

若：

$S_m > S$

則實際不含勻加速段及勻減速段，需重新計算最大速度，這種情況使用二分法，參考本博客中5段S形加減速中的方法計算即可