一文教你快速搞懂速度曲線規划之T形曲線（超詳細+圖文+推導+附件代碼）

 

運動控制中常用的T速度曲線規划的原理和程序實現，最后給出了測試結果；
如果本文幫到了您，請幫忙點個贊 👍👍👍；
如果本文幫到了您，請幫忙點個贊 👍👍👍；
如果本文幫到了您，請幫忙點個贊 👍👍👍；

 

文章目錄

• 1 前言
• 2 理論分析
• 3 matlab 實現
• 4 測試結果
• 5 c語言實現
• 6 總結

 

1 前言

在伺服系統以及控制系統的加減速動作中，為了讓速度更加平滑，可以引入T型速度曲線規划（T-curve velocity profile），T曲線是工業界廣泛采用的形式，它是一種時間最優的曲線。一般情況，曲線加速和減速的過程是對稱的，設給定速度上限為$v^{max}$。加速度上限為$a^{max}$，被控對象從A點運動到B點，要求生成的軌跡在這些條件下時間最優¹。

2 理論分析

在整體系統高速啟動，制動的狀態下，可以提高整體系統的性能。每當系統完成一個動作的時候，總共包括三個過程，勻加速，勻速，勻減速，具體如下圖所示；

根據$v$是否到達$v^{max}$，這里通常要分為兩種情況來討論；

• 第一種：速度到達$v^{max}$，最終速度曲線為梯形；
• 第二種：速度沒有到達$v^{max}$，最終速度曲線為三角形；

下面僅討論第一種情況；

這里時間使用$t$加腳標來表示，位置量使用$p$來表示，加速度使用$a$來表示

• 設加速時間長度為$t^a$：$t^0—t^1$；
• 因為加速和減速的過程是對稱的，所以減速帶的時間長度也為$t^a$：$t^2—t^3$；
• 最大速度$v^{max}$c持續的時間長度為$t^m$：$t^1—t^2$；

在實際的系統中，梯形曲線通常需要設置三個參數：

1. 最大速度$v^{max}$；
2. 加速度$a^{max}$；
3. 最終位置值$P^{final}$，下面簡稱為$P^f$；

所以這三個參數可以作為已知量來處理；

下面簡單推到這三個參數之間的關系：
設加減速區域經過的位置量為$P^a$,則：
$$P^a=\genfrac{}{}{0ex}{}{1}{2}{a}^m{t}^a$$

設最大區域經過的位置量為$P^m$,則：
$$\{\begin{array}{l}{P}^m=v^m{t}^m\cdots ①\\ P^f=P^a+P^m+P^a\cdots ②\\ t^a=\genfrac{}{}{0ex}{}{v^{max}}{a^{max}}\cdots ③\\ t^m=\genfrac{}{}{0ex}{}{(P^f-2P^a)}{v^{max}}\cdots ④\end{array}$$
所以輸出的位置量滿足以下關系：
$$P(t)=\{\begin{array}{l}\genfrac{}{}{0ex}{}{1}{2}{a}^m{t}^2，t^0\le t\le t^1\\ \genfrac{}{}{0ex}{}{1}{2}{a}^m{t}^a+v^m(t-t^a),t^1<t\le t^2\\ \genfrac{}{}{0ex}{}{1}{2}{a}^m{t}^a+v^m{t}^m+\genfrac{}{}{0ex}{}{1}{2}{a}^m(t-t^m-t^a{)}^2,t^2<t\le t^3\end{array}$$
最終可以通過$P(t)$的關系以及①②③④式編寫程序得到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曲線就可以減少這種情況的發生。本文寫的相對比較簡單，筆者能力有限，難免出現錯誤和紕漏，希望大佬不吝賜教。

文中難免有錯誤和紕漏之處，請大佬們不吝賜教
創作不易，如果本文幫到了您；
請幫忙點個贊 👍👍👍；
請幫忙點個贊 👍👍👍；
請幫忙點個贊 👍👍👍；

---

1. 《S/T曲線速度規划在定點DSP上的實現》 ↩︎