梯形速度规划算法
梯形速度是运动控制系统使用的最简单的一种速度规划方式，其速度曲线如下图所示：
v0为初速度，vn为匀速运行速度，vt为末速度，加速和减速阶段加速度为a，总路程为S。
则加速阶段走过的路程S1=(vn*vn-v0*v0)/2a,减速阶段走过的路程S3=(vn*vn-vt*vt)/2a，
则匀速阶段走过的路程S2=S-S1-S2
若S2<=0，代表此时没有匀速阶段，此时梯形速度退化成下图：
加速段和减速段相交点位最大速度Vm，由下列关系式成立：
则推导出：
带入算得：
（1-1）
S1为加速阶段走过的总路程，若S1<0，代表没有加速阶段只有减速阶段，若S2>S，代表只有加速阶段没有减速阶段。
最后总结一下梯形加速度规划方案：
在规划函数中：
在速度生成函数中：