Double S 曲线轨迹规划——不同速度加速度条件下综合

目前机械臂常用的轨迹曲线主要是S曲线，由于前后两端速度为抛物线形式，整体相对平滑，具有较好的过渡。程序涵盖了混合轨迹，加速轨迹和减速轨迹，通过设置flag和合理的速度加速度输入值获取双S曲线。

参数的定义

曲线不同段参数的定义由以下说明确定：参考《Trajectory Planning for Automatic Machine and Robots》P81。方程和框图均来自原文。

不同情况的分析

（1）Vlim=Vmax

即最大速度达到限定速度，在这种情况下，通过下式来判断加速度（减速度值）值是否达到了最大值：

判断之后，根据下式继续求出加速段时间的间隔Tj1:

否则，如果没有达到我们设定的加速度最大值，即继续处于加速段，则我们使用另一个式子求出加速时间Ta：

同理，减速区域采用同样的算法进行设计，如下式子可以进一步求出：

经过上面的计算之后，我们就能确定恒速段的时间Tv：

当计算获取Tv>0，那么说明我们设计的速度已经达到了最大速度，并通过上述式子解算获取。但是，可如果我们计算的Tv<0，那么在这种情况下，说明我们实际速度Vmax大于了我们的限定速度Vlim，在这种情况下，我们需要另做考虑和处理。

（2）Vlim<Vmax

即最大速度超过了我们设定的限制速度Vlim，在这种情况下就，不存在恒速度。也即Tv=0,。在这种情况下我们需要进一步分成三种情况来考虑。也即混合轨迹（包含了加速度和减速度轨迹），单加速度轨迹，和减速度轨迹这三种情况。下面分别进行介绍

①混合轨迹

混合轨迹是在规划轨迹中同时存在加速段和减速度轨迹。当加速度和减速度的最大值都达到了我们设计的最大值max，我们可以通过下式计算加减速轨迹段的时间Tj

由于加减速不一定刚好对分，这样的不确定因素对求解其中的参数会比较困难，这里通过引入变量delta，通过逐步缩减amax值的方法，amax = γamax（ 0 < γ < 1)来计算规划轨迹的加减速段的时间Ta,Td。通过计算条件：
Ta > 2Tj and Td > 2Tj 均满足时停止减少amax值.

而在上述计算过程中，还可能会出现Ta,Td为负值的情况，这种情况下，则是该规划轨迹仅仅有加速段轨迹或者仅仅有减速度轨迹。
在这种情况下，速度给定的初始值V0和V1的关系：v0>v1是计算成立的一个必要条件。

②单减速段轨迹

v0>v1，Ta<0时，不存在加速段轨迹。

③ 单加速段轨迹

v0>v1，Td<0时，不存在加速段轨迹。

最大（加）速度计算

在确定了整个轨迹不同段的运行时间后，我们就可以的计算轨迹中的最大限定加减速度alima,alimd，和整个轨迹的最大限定速度vlim。由下式确定：

Double S 轨迹生成

在通过上述方程计算出每段轨迹的持续时间和最大限定速度后，我们接着通过下面的方程开始正式进行Double S轨迹的生成计算。S轨迹也称作七段轨迹曲线，因此主要由七段函数构成。
注意：在生成轨迹的过程中还要注意初始位置条件q1和q0的关系，二者关系的不同分为两种情况：

（1）q1>q0

（2）q1<q0

在这种情况下，我们重点需要考虑速度和位置的符号相反（正负）性。通常情况下，在给定的任意初始速度和加速度条件，在计算轨迹曲线时我们常常将其前面乘以一个参量sigma，如下：

其中，

而同样，加速度速度等轨迹参数同时进行变换：

最后，也要特别注意在计算S曲线轮廓完成后，必须重现转换：转化后得到真实的规划的Double S轨迹曲线。

Double S曲线轨迹轮廓综合实现

通过以上的分析，其实可以将其综合成通用的轨迹轮廓生成函数块从而方便调用。程序框图如下图所示。

clc;clear all;
%%
%NOTE: 速度轨迹规划初步实现
%① Vlimt=Vmax时，Tv>0；
%② Vlimt<Vmax时，Tv=0；Tv，velocity constant period
%                1）加减速混合轨迹；
%                2）仅加速轨迹 v0<v1；
%                3）仅减速轨迹 v0>v1；%% 输入参数
q0=0;q1=10;v0=0;v1=-5;
vmax=5;amax=10;jmax=30;
n=1000 %总点数%% 计算程序
[ta,td,tv,tj1,tj2,vlim,alima,alimd,jmax]=myfunC(q0,q1,v0,v1,vmax,amax,jmax,8)
duration=ta+td+tv;
dt=duration/n;
[uv,vel,acc,jerk]=DoubleS(n,q0,v0,q1,v1,ta,td,tv,tj1,tj2,vlim,alima,alimd,jmax,duration);%% 绘图
myplot(uv,dt,duration,vel,vmax,acc,amax,jerk,jmax)%——————————————————————————————————————————————————————————————————%
%% matlab 函数库
%% 绘图
function myplot(uv,dt,duration,vel,vmax,acc,amax,jerk,jmax)
t=dt:dt:duration;
subplot(4,1,1)
plot(t,uv,'r-','linewidth',1.5);
title('位移曲线');xlabel('时间t[s]');ylabel('位移[rad]');grid on;
subplot(4,1,2)
plot(t,vel,'b','linewidth',1.5);
hold on;plot([0,duration],[vmax,vmax],'m--');plot([0,duration],[-vmax,-vmax],'g--');hold off;
title('速度曲线');xlabel('时间t[s]');ylabel('速度[rad/s]');grid on;legend('vel','vmax','vmin')
subplot(4,1,3)
plot(t,acc,'g-','linewidth',1.5)
hold on;plot([0,duration],[amax,amax],'m--');plot([0,duration],[-amax,-amax],'g--');hold off;
title('加速度曲线');xlabel('时间t[s]');ylabel('加速度[rad/s^2]');grid on;legend('acc','amax','amin')
subplot(4,1,4)
plot(t,jerk,'k-.','linewidth',1.5)
hold on;plot([0,duration],[jmax,jmax],'m--');plot([0,duration],[-jmax,-jmax],'g--');hold off;
title('jerk曲线');xlabel('时间t[s]');ylabel('加加速[rad/s^3]');grid on;legend('jerk','jmax','jmin')end
%%
function  [uv,vel,acc,jerk]=DoubleS(n,q0,v0,q1,v1,ta,td,tv,tj1,tj2,vlim,alima,alimd,jmax,duration)
uv=zeros(1,n);
vel=zeros(1,n);
acc=zeros(1,n);
jerk=zeros(1,n);
h=q1-q0;        %desired displacement h
jmin=-jmax;% vlim
% alimd=-alima;
for l=1:1:n%t=(l*duration-1)/(n);t=duration*(l-1)/(n-1);% 1if t<=tj1uv(l)= q0 + v0*t + jmax/6*t^3;vel(l)=v0 + jmax/2*t^2;acc(l)=jmax*t;jerk(l)=jmax;% 2 (无匀速段时可省略)elseif  (t>tj1 )&&( t<ta-tj1)uv(l)= q0 + v0*t + alima/6*(3*t^2-3*tj1*t+tj1^2);vel(l)=v0 + alima*(t-tj1/2);acc(l)=alima;jerk(l)=0;% 3elseif  (t>ta-tj1) && (t<=ta )uv(l)= q0 +1.0/2*(vlim+v0)*ta - vlim*(ta-t) - 1.0/6*jmin*(ta-t)^3;vel(l)=vlim + 1.0/2*jmin*(ta-t)^2;acc(l)= - jmin*(ta-t);jerk(l)=jmin;% 4 ()elseif  (t>ta) && (t<=ta+tv )uv(l)= q0 + 1.0/2*(vlim+v0)*ta + vlim*(t-ta);vel(l)=vlim;acc(l)=0;jerk(l)=0;% 5elseif  (t>duration-td) &&  (t<=duration-td+tj2 )uv(l)= q1 - (vlim+v1)*td/2 + vlim*(t-duration+td) - jmax/6*(t-duration+td)^3;vel(l)=vlim - jmax/2*(t-duration+td)^2;acc(l)= - jmax*(t-duration+td);jerk(l)= - jmax;% 6elseif ( t>duration-td+tj2) && (t<=duration-tj2 )uv(l)= q1 - v1*(duration-t) + alimd/6*(3*(duration-t)^2-3*tj2*(duration-t)+tj2^2);vel(l)= vlim + alimd*(t-duration+td-tj2/2);acc(l)=- jmax*tj2;jerk(l)=0;% 7elseif  (t>duration-tj2) && (t<=duration )uv(l)= q1 - v1*(duration-t) - jmax/6*(duration-t)^3;vel(l)=v1 + jmax/2*(duration-t)^2;acc(l)=- jmax*(duration-t);jerk(l)=jmax;end
end
end%% 计算不同情况
function[ta,td,tv,tj1,tj2,vlim,alima,alimd,jmax]=myfunC(q0,q1,v0,v1,vmax,amax,jmax,flag)
if (vmax-v0)*jmax<amax^2    %amax is not reachedtj1=sqrt((vmax-v0)/jmax)ta=2*tj1
else                        %amax is  reachedtj1=amax/jmaxta=tj1+(vmax-v0)/amax
endif (vmax-v1)*jmax<amax^2    %amin is not reachedtj2=sqrt((vmax-v1)/jmax)td=2*tj2
else                       %amin is  reachedtj2=amax/jmaxtd=tj2+(vmax-v1)/amax
endtv=(q1-q0)/vmax-ta/2*(1+v0/vmax)-td/2*(1+v1/vmax)if tv>0vlim=vmax; % Case 1:vlimit=vmaxalima=jmax*tj1alimd=-jmax*tj2
elseif tv<0 % Case 2:vlimit<vmaxtv=0tj1=amax/jmaxtj2=tj1tj=tj1%     deat=amax^4/(jmax^2)+2*(v0^2+v1^2)+amax*(4*(q1-q0)-2*amax/jmax*(v0+v1))
endswitch flagcase 0   %混合轨迹deat=amax^4/(jmax^2)+2*(v0^2+v1^2)+amax*(4*(q1-q0)-2*amax/jmax*(v0+v1))ta=(amax^2/jmax-2*v0+sqrt(deat))/(2*amax)%两个解%     ta=(amax^2/jmax-2*v0-sqrt(deat))/(2*amax)   %ta<0td=(amax^2/jmax-2*v1+sqrt(deat))/(2*amax)%两个解%     td=(amax^2/jmax-2*v1-sqrt(deat))/(2*amax)   %td<0case 1   %仅减速轨迹deat=amax^4/(jmax^2)+2*(v0^2+v1^2)+amax*(4*(q1-q0)-2*amax/jmax*(v0+v1))ta=(amax^2/jmax-2*v0-sqrt(deat))/(2*amax)   %ta<0td=(amax^2/jmax-2*v1+sqrt(deat))/(2*amax)%两个解case 2deat=amax^4/(jmax^2)+2*(v0^2+v1^2)+amax*(4*(q1-q0)-2*amax/jmax*(v0+v1))ta=(amax^2/jmax-2*v0+sqrt(deat))/(2*amax)%两个解td=(amax^2/jmax-2*v1-sqrt(deat))/(2*amax)   %td<0otherwisereturn;
endvlim=(q1-q0)/td;
alima=jmax*tj1;
alimd=-jmax*tj2;
vlim=v0+(ta-tj1)*alima;
vlim=v1-(td-tj2)*alimd;%仅有加减速的情况
if (ta<0 && v0>v1) % Case 2-1:ta<0 &v0>v1 只有减速段ta=0td=2*(q1-q0)/(v1+v0)tj1=0tj2=(jmax*(q1-q0)-sqrt(jmax*(jmax*(q1-q0)^2+(v1+v0)^2*(v1-v0))))/(jmax*(v1+v0))tv=0alima=jmax*tj1alimd=-jmax*tj2% vlim=v0+(ta-tj1)*alimavlim=v1-(td-tj2)*alimd
end
if (td<0 && v0<v1) % Case 2-1:td<0 &v0<v1 只有加速段tv=0td=0ta=2*(q1-q0)/(v1+v0)tj2=0tj1=(jmax*(q1-q0)-sqrt(jmax*(jmax*(q1-q0)^2-(v1+v0)^2*(v1-v0))))/(jmax*(v1+v0))alima=jmax*tj1alimd=-jmax*tj2vlim=v0+(ta-tj1)*alima%         vlim=v1-(td-tj2)*alimd
end
end

输出结果：