運動控制中常用的T速度曲線規劃的原理和程序實現,最後給出了測試結果;
如果本文幫到了您,請幫忙點個贊 👍👍👍;
如果本文幫到了您,請幫忙點個贊 👍👍👍;
如果本文幫到了您,請幫忙點個贊 👍👍👍;
1 前言
在伺服系統以及控制系統的加減速動作中,爲了讓速度更加平滑,可以引入T型速度曲線規劃(T-curve velocity profile),T曲線是工業界廣泛採用的形式,它是一種時間最優的曲線。一般情況,曲線加速和減速的過程是對稱的,設給定速度上限爲。加速度上限爲,被控對象從A點運動到B點,要求生成的軌跡在這些條件下時間最優1。
2 理論分析
在整體系統高速啓動,制動的狀態下,可以提高整體系統的性能。每當系統完成一個動作的時候,總共包括三個過程,勻加速,勻速,勻減速,具體如下圖所示;
根據是否到達,這裏通常要分爲兩種情況來討論;
- 第一種:速度到達,最終速度曲線爲梯形;
- 第二種:速度沒有到達,最終速度曲線爲三角形;
下面僅討論第一種情況;
這裏時間使用加腳標來表示,位置量使用來表示,加速度使用來表示
- 設加速時間長度爲:;
- 因爲加速和減速的過程是對稱的,所以減速帶的時間長度也爲:;
- 最大速度c持續的時間長度爲:;
在實際的系統中,梯形曲線通常需要設置三個參數:
- 最大速度;
- 加速度;
- 最終位置值,下面簡稱爲;
所以這三個參數可以作爲已知量來處理;
下面簡單推到這三個參數之間的關係:
設加減速區域經過的位置量爲,則:
設最大區域經過的位置量爲,則:
所以輸出的位置量滿足以下關係:
最終可以通過的關係以及①②③④式編寫程序得到T型速度曲線規劃。
3 matlab 實現
matlab的算法實現如下;
%% 梯形速度曲線
%% https://blog.csdn.net/u010632165
% Vm 最大熟讀
% Am 最大加速度
% P 位置信號
%%
function t_curve(Vm,Am,P)
%設置初始條件
t0=0;
P0=0;
Pf=P; %最終位置
v_max=Vm; %最大速度
a_max=Am; %最大加速度
ta=v_max/a_max; %加速和減速需要的時間
Pa=0.5*a_max*ta^2; %加速或減速產生的位置量
t_m=(Pf-2*Pa)/v_max;%最大速度需要的時間
t_f=t_m+2*ta; %到達目標位置所需要的時間
t=t0:0.1:t_f;
n=size(t);
Pt=zeros(n(2),1);
i=1;
% 判斷速度曲線規劃屬於哪一種情況
if t_f-2*ta>0
%達到最大速度,梯形
for t=t0:0.1:t_f
if t<=ta
Pt(i)=P0+0.5*a_max*t*t;
elseif ta<t && t<=t_f-ta
Pt(i)=P0+0.5*a_max*ta*ta+a_max*ta*(t-ta);
else
Pt(i)=Pf-0.5*a_max*(t_f-t)^2;
end
i=i+1;
end
else
% 未達到最大速度,速度曲線爲三角形
ta=sqrt( (Pf-P0)/a_max);
t_f=2*ta;
for t=t0:0.1:t_f
if t<=ta
Pt(i)=P0+0.5*a_max*t*t;
else
Pt(i)=Pf-0.5*a_max*(t_f-t)^2;
end
i=i+1;
end
end
subplot(3,1,1);
plot(Pt);
legend('位置曲線')
subplot(3,1,2);
plot(diff(Pt))
legend('速度曲線')
subplot(3,1,3);
plot(diff(diff(Pt)))
legend('加速度曲線')
end
4 測試結果
在matlab的命令終端輸入以下指令;
t_curve(3,1,20)
設置最大速度爲3,加速度爲1,最終位置爲20;
仿真曲線如下所示;
5 c語言實現
在simulink
中調用了c
程序進行仿真測試,《一文教你快速學會在matlab的simulink中調用C語言進行仿真 》具體代碼如下所示;
void sfun_myc_Outputs_wrapper(const real_T *u0,
const real_T *u1,
const real_T *u2,
const real_T *t,
real_T *y0,
real_T *y1,
real_T *y2)
{
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_BEGIN --- EDIT HERE TO _END */
/* This sample sets the output equal to the input
y0[0] = u0[0];
For complex signals use: y0[0].re = u0[0].re;
y0[0].im = u0[0].im;
y1[0].re = u1[0].re;
y1[0].im = u1[0].im;
*/
/* %%%-SFUNWIZ_wrapper_Outputs_Changes_END --- EDIT HERE TO _BEGIN */
int Am = u0[0];
int Vm = u1[0];
int Pf = u2[0];
int T = t[0];
int Ta = Vm/Am;
int Tm = (Pf - Am*Ta*Ta)/Vm;
int Tf = 2*Ta+Tm;
printf("%d\r\n",Tf);
//梯形
if(Tm>0){
if(T <= Ta){
y0[0] = 0.5*Am*T*T;
y1[0] = Am*T;
y2[0] = Am;
}else if(T<=(Ta+Tm)){
y0[0] = 0.5*Am*Ta*Ta + Vm*(T-Ta);
y1[0] = Vm;
y2[0] = 0;
}else if(T<=(Ta+Tm+Ta)){
y0[0] = 0.5*Am*Ta*Ta + Vm*Tm + 0.5*Am*(T-Ta-Tm)*(T-Ta-Tm);
y1[0] = Vm-Am*(T-Ta-Tm);
y2[0] = -Am;
}
}else{
//三角形
Ta = sqrt(Pf/Am);
if(T<Ta){
y0[0] = 0.5*Am*T*T;
y1[0] = Am*T;
y2[0] = Am;
}else{
y0[0] = 0.5*Am*Ta*Ta + 0.5*Am*(T-Ta)*(T-Ta);
y1[0] = Am*Ta - Am*(T-Ta);
y2[0] = -Am;
}
}
}
仿真結果如下;
6 總結
T曲線是工業界廣泛採用的形式,在運動控制上,相比較S曲線,它算法的複雜度更低,所佔用的系統資源更少,但是在恆加速的拐點會出現過沖,這裏S曲線就可以減少這種情況的發生。本文寫的相對比較簡單,筆者能力有限,難免出現錯誤和紕漏,希望大佬不吝賜教。
文中難免有錯誤和紕漏之處,請大佬們不吝賜教
創作不易,如果本文幫到了您;
請幫忙點個贊 👍👍👍;
請幫忙點個贊 👍👍👍;
請幫忙點個贊 👍👍👍;
《S/T曲線速度規劃在定點DSP上的實現》 ↩︎