機器人學回爐重造(5-2):關節空間規劃方法——梯形加減速(與拋物線擬合的線性函數)、S型曲線規劃

寫在前面

前面的幾種多項式規劃方法機器人學回爐重造(5):關節空間規劃方法——多項式軌跡(三次多項式、五次多項式、拋物線軌跡)都存在沒有最大速度和最大加速度限制的問題,當機械繫統存在速度和加速度限制範圍時,上述方法就不太適合。下面的梯形加減速和S型曲線就不會存在這種問題。

梯形加減速(與拋物線擬合的線性函數)

在這裏插入圖片描述
梯形曲線在電機控制中經常被用到,因爲其速度曲線形狀爲梯形,因此被稱爲梯形速度曲線。
一般情況下,用戶給定起始速度、終止速度、加速度、減速度、最大速度以及起始時刻和終止時刻的位移參數,需要計算出加速段、勻速段以及減速段對應的時間Ta、Tv、Td,然後再根據位移、速度以及加速度公式計算軌跡。
(1)首先需要計算用戶給定參數下所能達到的最大速度。要使能夠達到的速度最大,則只有加速段和減速段,沒有勻速段。此時h = p1 - p0,有如下公式
在這裏插入圖片描述
根據上述公式,可得到給定參數下所能達到的最大速度:
在這裏插入圖片描述
(2)比較理論最大速度vf與用戶限定最大速度vmax之間的大小。當vf<vmax時,說明系統能夠達到的最大速度無法滿足用戶要求,此時勻速段的最大速度爲vv = vf;當vf>vmax時,說明給定的參數可以達到用戶限定的最大速度,但是又不能超過限定的最大速度,因此勻速段的最大速度爲vv = vmax
(3)計算加速段、勻速段以及減速段的時間和位移,計算公式如下:
在這裏插入圖片描述
(4)計算各時刻下的位移、速度以及加速度,公式如下:
在這裏插入圖片描述

%% 與拋物線擬合的線性函數(線性軌跡)
% 梯形加減速,可限制最大速度和最大加速度
% 用戶給定起始速度、終止速度、加速度、減速度、最大速度及位移參數
% 該算法需要計算出加速段、勻速段以及減速段對應的時間Ta Tv Td
% t0 = 0, p0 = 5, p1 = 30, v0 = 50, vmax = 150, v1 = 20, aa = 1000
% ad = -1500
t0 = 2;
p0 = 5; p1 = 30;
v0 = 50; vmax = 150; v1 = 20;
aa = 1000; ad = -1500;
h = p1 - p0;
% 可達到的最大速度
vf = sqrt((2.0*aa*ad*h - aa*v1^2 + ad*v0^2) / (ad - aa));
% 確定勻速階段速度
if (vf < vmax)
    vv = vf;
else
    vv = vmax;
end
% 計算加速階段的時間和位移
Ta = (vv - v0) / aa;
La = v0*Ta + (1.0/2.0)*aa*Ta^2;
% 計算勻速階段的時間和位移
Tv = (h - (vv^2 - v0^2)/(2.0*aa) - (v1^2 - vv^2)/(2.0*ad)) / vv;
Lv = vv*Tv;
% 計算減速階段的時間和位移
Td = (v1 - vv) / ad;
Ld = vv*Td + (1.0/2.0)*ad*Td^2;
k = 1;
ts = 0.001;
% 計算軌跡的離散點
for t = t0: ts: (t0+Ta+Tv+Td)
    time(k) = t;
    t = t - t0;
    if (t >= 0 && t < Ta)
        p(k) = p0 + v0*t + (1.0/2.0)*aa*t^2;
        pd(k) = v0 + aa*t;
        pdd(k) = aa;
    elseif (t >= Ta && t < Ta+Tv)
        p(k) = p0 + La + vv*(t - Ta);
        pd(k) = vv;
        pdd(k) = 0;
    elseif (t >= Ta+Tv && t <= Ta+Tv+Td)
        p(k) = p0 + La + Lv + vv*(t - Ta - Tv) + (1.0/2.0)*ad*power(t - Ta - Tv, 2);
        pd(k) = vv + ad*(t - Ta - Tv);
        pdd(k) = ad;
    end
    k = k + 1;
end
figure(1)
subplot(3, 1, 1)
plot(time, p, 'r', 'LineWidth', 1.5)
ylabel('position')
grid on
subplot(3, 1, 2)
plot(time, pd, 'b', 'LineWidth', 1.5)
ylabel('velocity')
grid on
subplot(3, 1, 3)
plot(time, pdd, 'g', 'LineWidth', 1.5)
ylabel('acceleration')
grid on

在這裏插入圖片描述
由上圖可以看出,梯形曲線在t0、Ta、Ta+Tv以及Ta+Tv+Td時刻時加速度不連續,速度過渡不平滑,存在衝擊,這是梯形曲線的缺點,因此該加減速曲線常用於低速、低成本的運動控制過程。下面的S型曲線能解決這個問題。

S型曲線

在這裏插入圖片描述
定義:

  • 加速段 0 <= t < Ta:
    • 以恆定的jerk J(加速度的導數)使加速度從0增加到預先設定的a;
    • 以恆定的加速度加速;
    • 已恆定的負的jerk J(加速度的導數)使加速度從預先設定的a減到0;
  • 最大速度段 Ta <= t < Ta + Tv:
    • 以恆定的速度v勻速運動;
  • 減速段 Ta + Tv <= t <= Ta + Tv + Td:
    • 已恆定的負的jerk J(加速度的導數)使加速度從0減到預先設定的-a;
    • 以恆定的加速度-a減速;
    • 以恆定的jerk J(加速度的導數)使加速度從預先設定的-a增加到0;

總的運行段數爲七段,因此S型曲線也成爲七段式曲線。

下面是S曲線的算法步驟:

第一步:由於初始時刻和終止時刻的位置存在誰大誰小的問題,即q0<q1或者q0>q1,這會對最後的軌跡曲線造成影響。需經如下處理:
在這裏插入圖片描述
然後進行第二步;
第二步:判斷是否能夠達到最大速度,步驟如下:
在這裏插入圖片描述
在這裏插入圖片描述
此時得到勻速時間Tv,進行第三步;

第三步:如果Tv > 0,則跳轉到第四步;反之,則跳轉到第五步;

第四步:此時滿足條件Tv > 0,說明系統能夠達到的最大速度與設定的最大速度相等,即vlim = vmax,存在勻速時間,整理計算得到Ta、Tv、Td、Tj1、Tj2直接跳轉到第十一步計算軌跡;

第五步:此時滿足Tv <= 0,說明系統不能達到指定的最大速度vlim < vmax,系統不存在勻速段。此時Tv = 0。此時先假設系統能夠達到最大加速度和最小加速度,進行如下計算:
在這裏插入圖片描述
得到Ta、Td,對兩者進行討論,進行第六步;

第六步:判斷Ta <0 or Td < 0?,如果Ta < 0 || Td < 0,則跳轉第七步;反之,則跳轉至第八步;

第七步:滿足Ta < 0 || Td < 0的條件。如果此時Ta < 0,令Ta = 0,說明系統沒有加速度段,只有減速段,減速段參數利用如下公式計算:
在這裏插入圖片描述
此時得到Ta、Tj,跳轉至第十一步直接計算軌跡;
如果Td < 0,令Td = 0,說明系統沒有減速段,只有加速段,加速段參數利用如下公式計算:
在這裏插入圖片描述
此時得到Td、Tj,跳轉至第十一步直接計算軌跡;

第八步:判斷Ta >= 2Tj && Td >= 2Tj是否成立,如果成立則跳轉至第九步;反之,則跳轉至第十步;

第九步:此時Ta >= 2Tj && Td >= 2Tj,說明系統的加速段和減速段均能達到最大加速度,整理(3.26a)、(3.26b)、(3.26c)得到的Ta、Td和Tj,直接跳轉至第十一步計算軌跡;

第十步:系統加速和減速階段至少有一段不能達到最大加速度,這種情況很少見,處理方法爲逐漸減小amax,令amax = lambda*amax(0<lambda<1)(這種方法不是最優,但是從計算的角度可以接受),然後跳轉至第第五步進行循環,循環結束條件爲Ta >= 2Tj && Td >= 2Tj成立。這一步也體現了本文S曲線規劃的缺點,一旦出現第十步的情況,則無法獲得最優規劃曲線,也就是在指定的參數範圍內,無法獲得時間最短的軌跡。

第十一步:計算位置、速度、加速度以及加加速度軌跡曲線,公式如下,此時假設t0 = 0
在這裏插入圖片描述
在這裏插入圖片描述
在這裏插入圖片描述
在這裏插入圖片描述
在這裏插入圖片描述
在這裏插入圖片描述

S曲線流程圖

配合流程圖,更容易理解上述計算步驟:
在這裏插入圖片描述

Matlab實現程序

% 單關節空間規劃
%% S曲線參數計算(S型速度規劃,又稱七段式軌跡)
function para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax)
% 得到規劃參數Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin
% 用戶給定參數
% 邊界條件q0 = 10, q1 = 0, v0 = -7, v1 = 0
% 約束條件vmax = 10, amax = 10, jamx = 30
% q0 = 0; q1 = 10; 
% v0 = 7.5; v1 = 0;
% vmax = 10; amax = 10; jmax = 30;
vmin = -vmax; amin = -amax; jmin = -jmax;

%% 利用公式(3.31)(3.32)轉化得到實際的q_0、q_1、v_max、a_max
sigma = sign(q1 - q0);  
q_0 = sigma*q0;
q_1 = sigma*q1;
v_0 = sigma*v0;
v_1 = sigma*v1;
v_max = ((sigma+1)/2)*vmax + ((sigma-1)/2)*vmin;
v_min = ((sigma+1)/2)*vmin + ((sigma-1)/2)*vmax;
a_max = ((sigma+1)/2)*amax + ((sigma-1)/2)*amin;
a_min = ((sigma+1)/2)*amin + ((sigma-1)/2)*amax;
j_max = ((sigma+1)/2)*jmax + ((sigma-1)/2)*jmin;
j_min = ((sigma+1)/2)*jmin + ((sigma-1)/2)*jmax;

%% 判斷是否達到最大速度
if ((v_max - v_0)*j_max < a_max^2) 
    Tj1 = sqrt((v_max - v_0) / j_max); % 達不到a_max
    Ta = 2*Tj1;
    a_lima = j_max * Tj1;
else
    Tj1 = a_max / j_max; % 能夠達到a_max
    Ta = Tj1 + (v_max - v_0) / a_max;
    a_lima = a_max;
end
if ((v_max - v_1)*j_max < a_max^2)
    Tj2 = sqrt((v_max - v_1) / j_max); % 達不到a_min
    Td = 2*Tj2;
    a_limd =  -j_max * Tj2;
else
    Tj2 = a_max / j_max; % 能夠達到a_min
    Td = Tj2 + (v_max - v_1) / a_max;
    a_limd = -a_max;
end
% 根據(3.25)計算勻速段時間
Tv = (q_1 - q_0)/v_max - (Ta/2)*(1 + v_0/v_max) - (Td/2)*(1 + v_1/v_max);

%% 對Tv進行討論
if (Tv > 0)
    % 達到最大速度v_max,即存在勻速階段
    vlim = v_max;
    T = Ta + Tv + Td;
    para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
    return;
else
    % 達不到最大速度,即勻速階段Tv=0
    % 假設最大加速度和最小加速度均能達到
    Tv = 0;
    Tj = a_max / j_max;
    Tj1 = Tj;
    Tj2 = Tj;
    delta = (a_max^4/j_max^2) + 2*(v_0^2 + v_1^2) + a_max*(4*(q_1 - q_0) - 2*(a_max/j_max)*(v_0 + v_1));
    Ta = ((power(a_max, 2)/j_max) - 2.0*v_0 + sqrt(delta)) / (2.0*a_max);
    Td = ((power(a_max, 2)/j_max) - 2.0*v_1 + sqrt(delta)) / (2.0*a_max);
    % 對Ta和Td進行討論
    if (Ta < 0 || Td < 0)
        if (Ta < 0)
            % 沒有加速段,只有減速段
            Ta = 0; Tj1 = 0;
            Td = 2*(q_1 - q_0) / (v_0 + v_1);
            Tj2 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_max*power(q_1 - q_0, 2) + power(v_1 + v_0, 2)*(v_1 - v_0)))) / (j_max*(v_1 + v_0));
            a_lima = 0;
            a_limd = -j_max*Tj2;
            vlim = v0;
            para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
            return;
        elseif (Td < 0)
            % 沒有減速段,只有加速段
            Td = 0; Tj2 = 0;
            Ta = 2*(q_1 - q_0) / (v_0 + v_1);
            Tj1 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_max*power(q_1 - q_0, 2)) - power(v_1 + v_0, 2)*(v_1 - v_0))) / (j_max*(v_1 + v_0));
            a_lima = j_max*Tj1;
            a_limd = 0;
            vlim = v_0 + a_lima*(Ta - Tj1);
            para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
            return;
        end
    elseif (Ta >= 2*Tj && Td >= 2*Tj)
        % 加速段和減速段都能達到最大加速度
        a_lima = a_max;
        a_limd = -a_max;
        vlim = v0 + a_lima*(Ta - Tj);
        para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
        return;
    else
        % 加速和減速階段至少有一段不能達到最大加速度
        lambda = 0.99; % 系統取0<lambda<1
        while (Ta < 2*Tj || Td < 2*Tj)
            % 循環
            a_max = lambda*a_max;
            Tv = 0;
            Tj = a_max / j_max;
            Tj1 = Tj;
            Tj2 = Tj;
            delta = (a_max^4/j_max^2) + 2*(v_0^2 + v_1^2) + a_max*(4*(q_1 - q_0) - 2*(a_max/j_max)*(v_0 + v_1));
            Ta = ((power(a_max, 2)/j_max) - 2.0*v_0 + sqrt(delta)) / (2.0*a_max);
            Td = ((power(a_max, 2)/j_max) - 2.0*v_1 + sqrt(delta)) / (2.0*a_max);
            if (Ta < 0 || Td < 0)
                if (Ta < 0)
                    % 沒有加速段,只有減速段
                    Ta = 0; Tj1 = 0;
                    Td = 2*(q_1 - q_0) / (v_0 + v_1);
                    Tj2 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_max*power(q_1 - q_0, 2) + power(v_1 + v_0, 2)*(v_1 - v_0)))) / (j_max*(v_1 + v_0));
                    a_lima = 0;
                    a_limd = -j_max*Tj2;
                    vlim = v0;
                    para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
                    return;
                elseif (Td < 0)
                    % 沒有減速段,只有加速段
                    Td = 0; Tj2 = 0;
                    Ta = 2*(q_1 - q_0) / (v_0 + v_1);
                    Tj1 = (j_max*(q_1 - q_0) - sqrt(j_max*(j_max*power(q_1 - q_0, 2)) - power(v_1 + v_0, 2)*(v_1 - v_0))) / (j_max*(v_1 + v_0));
                    a_lima = j_max*Tj1;
                    a_limd = 0;
                    vlim = v_0 + a_lima*(Ta - Tj1);
                    para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
                    return;
                end
            elseif (Ta >= 2*Tj && Td >= 2*Tj)
                % 加速段和減速段都能達到最大加速度
                a_lima = a_max;
                a_limd = -a_max;
                vlim = v0 + a_lima*(Ta - Tj);
                para = [Ta, Tv, Td, Tj1, Tj2, q_0, q_1, v_0, v_1, vlim, a_max, a_min, a_lima, a_limd, j_max, j_min];
                return;
            end
        end
    end
end
end

%% 計算位移
function q = S_position(t, Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin)
T = Ta + Tv + Td;
% 加速段
if (t >= 0 && t < Tj1)
    q = q0 + v0*t + jmax*t^3/6;
elseif (t >= Tj1 && t < Ta - Tj1)
    q = q0 + v0*t +(alima/6)*(3*t^2 - 3*Tj1*t + Tj1^2);
elseif (t >= Ta - Tj1 && t < Ta)
    q = q0 + (vlim + v0)*(Ta/2) - vlim*(Ta - t) - jmin*((Ta - t)^3/6);
% 勻速段
elseif (t >= Ta && t < Ta + Tv)
    q = q0 + (vlim + v0)*(Ta/2) + vlim*(t - Ta);
% 減速段
elseif (t >= Ta + Tv && t < T - Td + Tj2)
    q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) - jmax*(power(t - T + Td, 3)/6);
elseif (t >= T - Td + Tj2 && t < T - Tj2)
    q = q1 - (vlim + v1)*(Td/2) + vlim*(t - T + Td) + (alimd/6)*(3*power(t - T + Td, 2) - 3*Tj2*(t - T + Td) + Tj2^2);
elseif (t >= T - Tj2 && t <= T)
    q = q1 - v1*(T - t) - jmax*(power(T - t, 3)/6);
end
end


%% 計算速度
function qd = S_velocity(t, Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin)
T = Ta + Tv + Td;
if (t >= 0 && t < Tj1)
    qd = v0 + jmax*(t^2/2);
elseif (t >= Tj1 && t < Ta - Tj1)
    qd = v0 + alima*(t - Tj1/2);
elseif (t >= Ta - Tj1 && t < Ta)
    qd = vlim + jmin*(power(Ta - t, 2)/2);
% 勻速段
elseif (t >= Ta && t < Ta + Tv)
    qd = vlim;
% 減速段
elseif (t >= Ta + Tv && t < T - Td + Tj2)
    qd = vlim - jmax*(power(t - T + Td, 2)/2);
elseif (t >= T - Td + Tj2 && t < T - Tj2)
    qd = vlim + alimd*(t - T + Td - Tj2/2);
elseif (t >= T - Tj2 && t <= T)
    qd = v1 + jmax*(power(t - T, 2)/2);
end
end


%% 計算加速度
function qdd = S_acceleration(t, Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin)
T = Ta + Tv + Td;
if (t >= 0 && t < Tj1)
    qdd = jmax*t;
elseif (t >= Tj1 && t < Ta - Tj1)
    qdd = alima;
elseif (t >= Ta - Tj1 && t < Ta)
    qdd = -jmin*(Ta - t);
% 勻速段
elseif (t >= Ta && t < Ta + Tv)
    qdd = 0;
% 減速段
elseif (t >= Ta + Tv && t < T - Td + Tj2)
    qdd = -jmax*(t - T + Td);
elseif (t >= T - Td + Tj2 && t < T - Tj2)
    qdd = alimd;
elseif (t >= T - Tj2 && t <= T)
    qdd = -jmax*(T - t);
end
end


%% 計算加加速度
function qddd = S_jerk(t, Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin)
T = Ta + Tv + Td;
if (t >= 0 && t < Tj1)
    qddd = jmax;
elseif (t >= Tj1 && t < Ta - Tj1)
    qddd = 0;
elseif (t >= Ta - Tj1 && t < Ta)
    qddd = jmin;
% 勻速段
elseif (t >= Ta && t < Ta + Tv)
    qddd = 0;
% 減速段
elseif (t >= Ta + Tv && t < T - Td + Tj2)
    qddd = -jmax;
elseif (t >= T - Td + Tj2 && t < T - Tj2)
    qddd = 0;
elseif (t >= T - Tj2 && t <= T)
    qddd = jmax;
end
end

測試程序

%% S曲線規劃
% 邊界條件
q0 = 0; q1 = 10; 
v0 = 1; v1 = 0;
vmax = 10; amax = 10; jmax = 30;
sigma = sign(q1 - q0);
% 得到規劃參數Ta, Tv, Td, Tj1, Tj2, q0, q1, v0, v1, vlim, amax, amin, alima, alimd, jmax, jmin
para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax)
i = 1; 
T = para(1) + para(2) + para(3)
for t = 0: 0.001: T
   time(i) = 0.001*i;
   q(i) = S_position(t, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
   qd(i) = S_velocity(t, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
   qdd(i) = S_acceleration(t, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
   qddd(i) = S_jerk(t, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
   i = i + 1;
end
q = sigma*q;
qd = sigma*qd;
qdd = sigma*qdd;
qddd = sigma*qddd;
figure(1)
subplot(4, 1, 1)
plot(time, q, 'r', 'LineWidth', 1.5)
grid on
subplot(4, 1, 2)
plot(time, qd, 'b', 'LineWidth', 1.5)
grid on
subplot(4, 1, 3)
plot(time, qdd, 'g', 'LineWidth', 1.5)
grid on
subplot(4, 1, 4)
plot(time, qddd, 'LineWidth', 1.5)
grid on

在這裏插入圖片描述


參考

S型速度規劃
Trajectory Planning for Automatic Machines and Robots


2019.11.27——發現兩處錯誤,STrajectoryPara.m中第86行、128行程序出錯,已改正爲vlim = v_0 + a_lima*(Ta - Tj1)

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章