天天看點
傳回

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃

文章目錄

    • 寫在前面
      • 學習代碼都記錄在[個人github](https://github.com/xuuyann/RobotLearningCode)上，歡迎關注~
    • 梯形加減速（與抛物線拟合的線性函數）
    • S型曲線
      • 下面是S曲線的算法步驟：
      • S曲線流程圖
      • Matlab實作程式
    • 參考

寫在前面

學習代碼都記錄在個人github上，歡迎關注~

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

梯形加減速（與抛物線拟合的線性函數）

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃

梯形曲線在電機控制中經常被用到，因為其速度曲線形狀為梯形，是以被稱為梯形速度曲線。

一般情況下，使用者給定起始速度、終止速度、加速度、減速度、最大速度以及起始時刻和終止時刻的位移參數，需要計算出加速段、勻速段以及減速段對應的時間Ta、Tv、Td，然後再根據位移、速度以及加速度公式計算軌迹。

（1）首先需要計算使用者給定參數下所能達到的最大速度。要使能夠達到的速度最大，則隻有加速段和減速段，沒有勻速段。此時

h = p1 - p0

，有如下公式

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃

根據上述公式，可得到給定參數下所能達到的最大速度：

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃

（2）比較理論最大速度vf與使用者限定最大速度vmax之間的大小。當

vf<vmax

時，說明系統能夠達到的最大速度無法滿足使用者要求，此時勻速段的最大速度為

vv = vf

；當

vf>vmax

時，說明給定的參數可以達到使用者限定的最大速度，但是又不能超過限定的最大速度，是以勻速段的最大速度為

vv = vmax

。

（3）計算加速段、勻速段以及減速段的時間和位移，計算公式如下：

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃

（4）計算各時刻下的位移、速度以及加速度，公式如下：

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃
%% 與抛物線拟合的線性函數（線性軌迹）
% 梯形加減速，可限制最大速度和最大加速度
% 使用者給定起始速度、終止速度、加速度、減速度、最大速度及位移參數
% 該算法需要計算出加速段、勻速段以及減速段對應的時間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
機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃

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

S型曲線

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、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，這會對最後的軌迹曲線造成影響。需經如下處理：

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃

然後進行第二步；

第二步：判斷是否能夠達到最大速度，步驟如下：

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃
機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃

此時得到勻速時間Tv，進行第三步；

第三步：如果Tv > 0，則跳轉到第四步；反之，則跳轉到第五步；

第四步：此時滿足條件Tv > 0，說明系統能夠達到的最大速度與設定的最大速度相等，即

vlim = vmax

，存在勻速時間，整理計算得到Ta、Tv、Td、Tj1、Tj2直接跳轉到第十一步計算軌迹；

第五步：此時滿足Tv <= 0，說明系統不能達到指定的最大速度

vlim < vmax

，系統不存在勻速段。此時

Tv = 0

。此時先假設系統能夠達到最大加速度和最小加速度，進行如下計算：

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃

得到Ta、Td，對兩者進行讨論，進行第六步；

第六步：判斷

Ta <0 or Td < 0?

，如果

Ta < 0 || Td < 0

，則跳轉第七步；反之，則跳轉至第八步；

第七步：滿足

Ta < 0 || Td < 0

的條件。如果此時

Ta < 0

，令

Ta = 0

，說明系統沒有加速度段，隻有減速段，減速段參數利用如下公式計算：

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃

此時得到Ta、Tj，跳轉至第十一步直接計算軌迹；

如果

Td < 0

，令

Td = 0

，說明系統沒有減速段，隻有加速段，加速段參數利用如下公式計算：

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃

此時得到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

：

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃
機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃
機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃
機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃
機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃
機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃

S曲線流程圖

配合流程圖，更容易了解上述計算步驟：

機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、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
機器人學回爐重造（5-2）：關節空間規劃方法——梯形加減速（與抛物線拟合的線性函數）、S型曲線規劃

參考

S型速度規劃

Trajectory Planning for Automatic Machines and Robots

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

機器人學 matlab 關節空間規劃 梯形加減速 S型速度規劃
上一篇: 3.CENTOS安裝CDH和Kudu-2
下一篇: java 漢字字典，擷取漢字拼音，拼音首字母，五筆，筆畫，筆畫順序

繼續閱讀