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

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

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

2. 七段式说明

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

3. S型曲线算法步骤

a) 首先判断能否达到最大速度

对于加速段，若达不到规划的最大加速度amax，则：

此时，加速段时间：

若加速段达到了规划的最大加速度amax，此时加速段时间：

                      对于减速段，若达不到规划的最大加速度-amax，则：

                       

                      此时，减速段时间：

                      

                      若减速段达到了规划的最大加速度-amax，此时加速段时间：

                       

                        b) 计算匀速段时间Tv, 对Tv进行分类讨论

                        

                          若Tv>0，则说明存在匀速时间，且能达到规划的最大速度和最大加速度，即vlim= vmax，alim= amax。则根据 步骤后面的公式计算轨迹参数。

                          若Tv=0,  则说明刚好没有匀速段，刚好能达到规划的最大速度和最大加速度。

                           

                          若Tv<0, 则说明不能达到规划的最大速度, 但是关于能否达到规划的最大加速度，还需讨论Ta与2Tj1，Td与2Tj2的大小，此时有：

                           

                             c) 讨论Ta与2Tj1，Td与2Tj2的大小

                              

                                d) 七段式公式

                                 

                                 

                                    4. matlab计算结果如下：