机器人运动学轨迹规划（三）----S型曲线规划算法

写在前面：机器人运动学轨迹规划（二）里介绍了T型曲线规划算法，本文主要介绍S型速度曲线算法。同T型速度曲线相比，S形曲线更加平滑，避免了T形曲线在速度拐点引起的电机和drive train的冲击，但是在相同的期望速度和加速度条件下，运动相同距离的时间要更长一点。

1. S型速度曲线一般包括加加速、匀加速、减加速、匀速、加减速、匀减速、减减速，一共7段，因此又被称为7段式曲线。下图是典型的S型曲线图。

2. 七段式说明

约束条件：q0和v0分别是起始点的位置和速度，q1和v1分别是终点的位置和速度。vmax, amax, jmax和分别是是规划的最大速度、最大加速度和最大加加速度。

3. S型曲线算法步骤

a)