電機(jī)運(yùn)動控制算法之S速度曲線規(guī)劃算法

轉(zhuǎn)自| 小麥大叔

搞過電機(jī)或運(yùn)動控制的小伙伴應(yīng)該知道，S曲線很重要，下面一張動圖對比一下，你就知道S曲線的好處。

今天就給大家描述一下S速度曲線規(guī)劃算法。

1 前言

S形加減速的最重要特征是該算法的加速度/減速度曲線的形狀如字母 S。S形加減速的速度曲線平滑 ,從而能夠減少對控制過程中的沖擊，并使插補(bǔ)過程具有柔性 [^1]。由于T形曲線在加速到勻速的切換過程中，實(shí)際中存在較大過沖，因此這里對比一下T曲線和7段S曲線的實(shí)際過程；

T形：加速 -> 勻速 -> 減速

S形：加加速(T1) -> 勻加速(T2) -> 減加速(T3)-> 勻速(T4)-> 加減速(T5)-> 勻減速(T6)-> 減減速(T7)

上文在加速這塊的文字描述可能讀起來起來有點(diǎn)繞，下面看圖：

2 理論分析

由于S曲線在加減速的過程中，其加速度是變化的，因此這里引入了新的一個(gè)變量 J，即加加速度。

因此對應(yīng)上圖的7段S速度曲線中，規(guī)定最大加速為amax，最小加速度為-amax，則加速度的關(guān)系；

所以通常需要確定三個(gè)最基本的系統(tǒng)參數(shù) :系統(tǒng)最大速度umax ，最大加速度a_{max} ，加加速度，就可以可確定整個(gè)運(yùn)行過程[^2] ；

最大速度：反映了系統(tǒng)的最大運(yùn)行能力 ；

最大加速度：反映了系統(tǒng)的最大加減速能力 ；

加加速度：反映了系統(tǒng)的柔性；

柔性越大，過沖越大，運(yùn)行時(shí)間越短；

柔性越小，過沖越小，運(yùn)行時(shí)間越長；

2.1 加速度時(shí)間關(guān)系方程

整個(gè)加速度變化的過程具體如下圖所示；

2.2 速度時(shí)間關(guān)系方程

速度和加速度滿足 ；加加速度和速度的關(guān)系滿足：

結(jié)合加速度時(shí)間關(guān)系并結(jié)合② 式可以得到速度曲線關(guān)系，具體關(guān)系如下圖所示；

進(jìn)一步簡化可以得到：

2.3 位移時(shí)間關(guān)系方程

積分忘的差不多了，回去再復(fù)習(xí)一下；

最終位移的方程如下所示；

3 程序?qū)崿F(xiàn)的思路

正如前面所提到的，S曲線規(guī)劃需要確定三個(gè)最基本的系統(tǒng)參數(shù) ：系統(tǒng)最大速度 ，最大加速度a_{max} ，加加速度，這樣就可以確定這個(gè)運(yùn)行過程。這里有一個(gè)隱性的條件，就是在運(yùn)行的過程中可以達(dá)到最大速度，這樣才是完整的7段S曲線，另外這里還有一些中間參數(shù)：

4 matlab 程序

matlab程序親測可以運(yùn)行，做了簡單的修改，因?yàn)檫@里直接給定了整個(gè)運(yùn)行過程的時(shí)間，所以需要在SCurvePara函數(shù)中求出加加速度 的值，路程為 1：

SCurvePara

function[Tf1,V,A,J,T]=SCurvePara(Tf,v,a)
T=zeros(1,7);
fori=1:1000
%加加速度J
J=(a^2*v)/(Tf*v*a-v^2-a);
%Tk
T(1)=a/J;
T(2)=v/a-a/J;%t2=v/a-t1;
T(3)=T(1);
T(4)=Tf-2*a/J-2*v/a;%t4=Tf-4*t1-2*t2;
T(5)=T(3);
T(6)=T(2);
T(7)=T(1);
%根據(jù)T2和T4判斷S曲線的類型
ifT(2)

	

	SCurveScaling

	
functions=SCurveScaling(t,V,A,J,T,Tf)
%J=(A^2*V)/(Tf*V*A-V^2-A);
%T(1)=A/J;
%T(2)=V/A-A/J;%T(2)=V/A-T(1);
%T(3)=T(1);
%T(4)=Tf-2*A/J-2*V/A;%T(4)=Tf-4*T(1)-2*T(2);
%T(5)=T(3);
%T(6)=T(2);
%T(7)=T(1);
%%
if(t>=0&&t<=?T(1))
????s?=?1/6?*?J?*?t^3;
elseif?(??t?>T(1)&&t<=?T(1)+T(2)?)
????dt?=?t?-?T(1);
????s?=?1/2?*?A?*?dt^2?+?A^2/(2*J)?*?dt...
????????+?A^3/(6*J^2);
elseif?(?t?>T(1)+T(2)&&t<=?T(1)+T(2)+T(3)?)
?????dt?=?t?-?T(1)?-?T(2);
?????s?=?-1/6*J*dt^3?+?1/2*A*dt^2?+?(A*T(2)?+?A^2/(2*J))*dt?...
?????????+?1/2*A*T(2)^2?+?A^2/(2*J)*T(2)?+?A^3/(6*J^2);
elseif?(?t?>T(1)+T(2)+T(3)&&t<=?T(1)+T(2)+T(3)+T(4)?)
?????dt?=?t?-?T(1)?-?T(2)?-?T(3);
?????s?=?V*dt?...
?????????+??(-1/6*J*T(3)^3)?+?1/2*A*T(3)^2?+?(A*T(2)?+?A^2/(2*J))*T(3)?+?1/2*A*T(2)^2?+?A^2/(2*J)*T(2)?+?A^3/(6*J^2);
elseif?(?t?>T(1)+T(2)+T(3)+T(4)&&t<=?T(1)+T(2)+T(3)+T(4)+T(5)?)
?????t_temp?=?Tf?-?t;?
?????dt?=?t_temp?-?T(1)?-?T(2);
?????s?=?-1/6*J*dt^3?+?1/2*A*dt^2?+?(A*T(2)?+?A^2/(2*J))*dt?...
?????????+?1/2*A*T(2)^2?+?A^2/(2*J)*T(2)?+?A^3/(6*J^2);
?????s?=?1?-?s;
elseif?(?t?>T(1)+T(2)+T(3)+T(4)+T(5)&&t<=?T(1)+T(2)+T(3)+T(4)+T(5)+T(6)?)
?????t_temp?=?Tf?-?t;?
?????dt?=?t_temp?-?T(1);
?????s?=?1/2?*?A?*?dt^2?+?A^2/(2*J)?*?dt?+?A^3/(6*J^2);
?????s?=?1?-?s;??
elseif?(?t?>T(1)+T(2)+T(3)+T(4)+T(5)+T(6)&&t<=?T(1)+T(2)+T(3)+T(4)+T(5)+T(6)+T(7)?+?1e5?)
?????t_temp?=?Tf?-?t;?
?????s?=?1/6?*?J?*?t_temp^3;
?????s?=?1?-?s;?????
end
?
end


	

	測試的代碼如下：TEST

	
%%
N=500;

ThetaStart=0;%起始位置
ThetaEnd=90;%最終位置
VTheta=90;%1速度
ATheta=135;%1.5加速度
Tf=1.8;%總行程時(shí)間

v=VTheta/(ThetaEnd-ThetaStart);
a=ATheta/(ThetaEnd-ThetaStart);
v=abs(v);
a=abs(a);


Theta=zeros(1,N);
s=zeros(1,N);
sd=zeros(1,N);
sdd=zeros(1,N);

[TF,V,A,J,T]=SCurvePara(Tf,v,a);
display(J,'J:');
display(TF,'Tf:');
display(V,'v:');
display(A,'da:');

display(TF-Tf,'dTf:');
display(V-v,'dv:');
display(A-a,'da:');

t=linspace(0,TF,N);
dt=t(2)-t(1);
fori=1:N
ifi==N
a=a;
end
s(i)=SCurveScaling(t(i),V,A,J,T,TF);
Theta(i)=ThetaStart+s(i)*(ThetaEnd-ThetaStart);
ifi>1
sd(i-1)=(s(i)-s(i-1))/dt;
end
ifi>2
sdd(i-2)=(sd(i-1)-sd(i-2))/dt;
end
end

subplot(3,1,1);
legend('Theta');
xlabel('t');
subplot(3,1,1);
plot(t,s)
legend('位移');
xlabel('t');
title('位置曲線');

subplot(3,1,2);
plot(t,sd);
legend('速度');
xlabel('t');
title('速度曲線');

subplot(3,1,3);
plot(t,sdd);
legend('加速度');
xlabel('t');
title('加速度曲線');



	

	看到最終仿真結(jié)果和預(yù)期相同；

	最后再看一下T形和S形速度曲線規(guī)劃的效果對比：

	5 總結(jié)

	本文只對7段的S曲線規(guī)劃做了詳細(xì)的推導(dǎo)和介紹，matlab中的程序?qū)τ?段和5段都有做實(shí)現(xiàn)，很多是在理想情況下進(jìn)行推導(dǎo)的，初始速度默認(rèn)為0，終止速度也為0，并且假設(shè)加減速區(qū)域相互對稱。最終運(yùn)行結(jié)果符合預(yù)期效果。

	審核編輯：湯梓紅