S形曲线规划方式汇总

本文主要讲述S形速度规划方案，包括sigmoid函数、sin函数和bezier曲线速度方案。已知初速度Vs,末速度Ve,规划速度曲线分别是sigmoid函数曲线、sin函数曲线、bezier曲线。

一 曲线速度规划方案

1 sigmoid函数曲线速度规划

这种方式在前面博文https://blog.csdn.net/liuzhijun301/article/details/103978087中已经讲述过，对于初速度为s，末速度为e，运动步数为2n的纯加/减速段，对于其中第i步，可以规划速度为：

                                                                

2 三角函数曲线速度规划

对于初速度为vs，末速度为ve，运动总步数为n的纯加减速段，对于其中第i步，规划速度为：

3 Bezier曲线速度规划

对于Bezier曲线的原理介绍见https://www.jianshu.com/p/8f82db9556d2

这里以开源cnc项目Marlin2.0中的s形速度规划举例。marlin2.0中采用了6点Bezier曲线。6点Bezier曲线是5阶形式：

V(t) = P_0 * B_0(t) + P_1 * B_1(t) + P_2 * B_2(t) + P_3 * B_3(t) + P_4 * B_4(t) + P_5 * B_5(t)，0

其中：

            B_0(t) =   (1-t)^5        =   -t^5 +  5t^4 - 10t^3 + 10t^2 -  5t   +   1

            B_1(t) =  5(1-t)^4 * t    =   5t^5 - 20t^4 + 30t^3 - 20t^2 +  5t

            B_2(t) = 10(1-t)^3 * t^2  = -10t^5 + 30t^4 - 30t^3 + 10t^2

            B_3(t) = 10(1-t)^2 * t^3  =  10t^5 - 20t^4 + 10t^3

            B_4(t) =  5(1-t)   * t^4  =  -5t^5 +  5t^4

            B_5(t) =             t^5  =    t^5

V（t）可以改写为：

V(t) = A*t^5 + B*t^4 + C*t^3 + D*t^2 + E*t + F

其中：

          A =    -P_0 +  5*P_1 - 10*P_2 + 10*P_3 -  5*P_4 +  P_5

         B =   5*P_0 - 20*P_1 + 30*P_2 - 20*P_3 +  5*P_4

         C = -10*P_0 + 30*P_1 - 30*P_2 + 10*P_3

         D =  10*P_0 - 20*P_1 + 10*P_2

         E = - 5*P_0 +  5*P_1

         F =     P_0

我们希望初始加速度和初始jerk都为0，因此我们设置P_i=P_0 = P_1 = P_2 (initial velocity），P_t = P_3 = P_4 = P_5 (target velocity)，经过简化以后有：

         A = - 6*P_i +  6*P_t =  6*(P_t - P_i)

         B =  15*P_i - 15*P_t = 15*(P_i - P_t)

         C = -10*P_i + 10*P_t = 10*(P_t - P_i)

         D = 0

         E = 0

         F = P_i

此时有 V(t) = A*t^5 + B*t^4 + C*t^3 + F          [0 <= t <= 1]

二 Matlab代码实现

matlab实现上述3种曲线规划代码：

clc;clear;
flex=5;
vmin=10;
vmax=100;
num=15;
for i=0:1:2*num+1
    t(i+1)=i;
    y(i+1)=vmin+(vmax-vmin)/(1+exp(-flex*(i-num)/num));%simoid
    z(i+1)=vmin+(vmax-vmin)/(2*num)*i;%梯形
    e(i+1)=vmin+(vmax-vmin)*(i/(2*num)-1/(2*pi)*sin(2*pi/(2*num)*i));%sin曲线
    f(i+1)=6*(vmax-vmin)*(i/(2*num))^5+15*(vmin-vmax)*(i/(2*num))^4+10*(vmax-vmin)*(i/(2*num))^3+vmin;%bezier曲线
end
plot(t,y,t,z,t,f)
hold on;grid on;legend

波形如下：

data1-4分别是sigmoid、梯形、sin、bezier曲线速度规划的速度曲线。

可以看到三种曲线形状差不多，但以bezier曲线最为柔和。运算量方面，sigmoid要执行e的幂运算所以较难优化，sin函数可以用转化成定点查表法进行优化，bezier曲线只涉及浮点乘法运算量速度相对sigmoid会较快，相对优化的sin函数则会较慢。下面画出sigmoid、sin和bezier三种曲线规划时的加速度曲线：

for i=0:1:2*num+1    
    if i>0
        err1(i+1)=(y(i+1)-y(i))/(1/(2*num));
        err2(i+1)=(e(i+1)-e(i))/(1/(2*num));
        err3(i+1)=(f(i+1)-f(i))/(1/(2*num));
    end
end
figure;
plot(t,err1,t,err2,t,err3);grid on;legend;

可见最大加速度最大依次是sigmoid>sin>bezier