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形加减速中的方法计算即可