其中:
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(initialvelocity),P_t=P_3=P_4=P_5(targetvelocity),经过简化以后有:
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